Documents

A Matlab Script to Recompute the Nonlinear Tracking Filter Examples 6

Description
A Matlab script to recompute the nonlinear tracking filter examples 6.1-2 & 6.1-3 in Gelb Part of my inverse theory research relies on concepts from recursive filters, so I had to take some time to come up to speed on those. A classic textbook for this is Applied Optimal Estimation, editted by Gelb (1974). In section 6.1 of that book are two simple radar tracking examples (6.1-2 and 6.1-3) which demonstrate several nonlinear filters. I've programmed up those examples into a Matlab script called
Categories
Published
of 8
All materials on our website are shared by users. If you have any questions about copyright issues, please report us to resolve them. We are always happy to assist you.
Related Documents
Share
Transcript
  A Matlab script to recompute the nonlinear trackingfilter examples 6.1-2 & 6.1-3 in Gelb Part of my inverse theory research relies on concepts from recursive filters, so I had totake some time to come up to speed on those. A classic textbook for this is  Applied Optimal Estimation , editted by Gelb (1974). In section 6.1 of that book are two simpleradar tracking examples (6.1-2 and 6.1-3) which demonstrate several nonlinear filters.I've programmed up those examples into a Matlab script calledgravdragdemo.mandadded a few other filters to compare and contrast them in both linear and nonlinearcases.These examples use radar ranging to estimate the elevation, downward velocity, anddrag coefficient for a falling body as functions of time. These three values are collectedinto a 3x1 vector called the state vector  x  , again a function of time. The two examplesare related: example 6.1-3 has a 2D arrangement to create nonlinear measurementswith respect to  x  . Example 6.1-2 is a special case of 6.1-3 in which the problem hascollapsed to 1D by letting r 1 and r 2 shrink to zero, causing the measurement relation tobecome linear with respect to  x  . The dynamics of both examples in the book arenonlinear because they include air drag (x 3 ), which depends on velocity (x 2 ). Here arerecreations of the figures in Gelb for examples 6.1-3 (2D nonlinear measurementscase) and example 6.1-2 (1D linear measurements case) respectively:   The equations for this problem are pretty basic: definition of state parameters:   dynamics equation:  (oops, note there should be a +v on each RHS inthis eqn, I'll update soon...) definition of drag d and atmospheric density relating to it:   measurement equation:  (oops, note there should be a +w on theRHS in this eqn, I'll update soon...)Note I actually don't redo the book's example 6.1-3 exactly; I still use x 3 = for both  examples, and use the initial conditions from example6.1-2 for both problems. Thislinked the two examples better, letting me simply zero out r 1 and r 2 to collapse 6.1-3down into 6.1-2.In the Matlab script we can choose whether to make the measurements a linear ornonlinear function of the states by setting r 1 and r 2 to be nonzero or zero. We can alsospecify whether to make the dynamics linear or nonlinear by setting the forces on thefalling body (drag d  and gravitational acceleration g ) to nonzero or zero. Granted, forthe linear case it's a little strange because the falling body no longer even hasgravity acting on it, but the idea was to have an example which kept at its initialvelocity to make it linear - so sue me!The point of running this script with its different settings is to compare the resultingplots showing how well the filter fared. A commonly seen plot in the filter community isthe estimation error plot, which for a synthetic example shows error deviations fromthe known true state vector  x  as a function of time. The filters have a random elementto them so the iterative paths toward a solution can jump around, but ideally thosepaths converge on the zero error line, meaning the filter converges to the true value.The fluctuating center line is the expected value (mean) state calculated by the filterminus the true state (only known in the synthetic problem), and the lines above andbelow forming a corral are the plus- and minus-one standard deviation lines. Here's anexample of what the plots look like, and we'll come back to discussthe details of thisparticular plot a little farther below.  An example of the estimation error plots the script uses to compare the filters for asynthetic problem:  We see at the top of the script which of the filters (or smoother) are in used in a givenrun:    % - Kalman Filter (KF),% - Kalman Smoother (KS),% - Linearized Kalman Filter (LKF),% - Extended Kalman Filter (EKF),% - 2nd order EKF (EKF2)  The initial c   ndition s for both exa m ¡  le s   6 ¢  1-2 and 6 ¢  1-3 here were taken fro m  exa m ¡  le s   6 ¢  1-2 in the book, and the initial c ovarian c e m atri c e s had to be e s ti m atedba s ed on the book 's figure s   6 ¢  1-3 and 6 ¢  1-4 ¢  I al s o added a Q m atrix option ( thebook 's exa m ple s a ss u m ed Q=0 and s o well leave the Q turned off for now ) ande s ti m ated the ti m e in c re m ent T fro m the book 's plot s , a s it wa s n ' t given :   % Initial conds and setup is the same for both 1D & 2D problems.% (these default values here are taken from book):x0true=[1e5+100,-6000+350,2500+500]'; % = elevation(ft),velocity(ft/sec), drag coeffx0guess=[1e5,-6000,2500]'; % note this will cause LKF's xbar tocross zero a little early...P0=diag([500,2e4,2.5e5Qon=0; % multiply this scalar times the Q matrix to turn iton&off (ie 1=on,0=off)% Gelb examples both have Q=0; you can change Q down in getQ()function, default is 1e2*I)R=1e2; % note meas noise cov matrix is a single scalar variance inthis problem% Note all filters in this script assume equally-spaced timepoints:T=.01; % Timestep for discretization of continuous functions oftime.% Note the Gelb book examples very likely used a timestep oforder% 0.01 and then only plotted every 10th point or so.timerange=0:T:16; % or try 17.7; % both in sec  It wa s   m entioned above that the sc ript c an handle both linear and nonlinear dyna m i cs  and m ea s ure m ent s for c o m pari s on of filter s ¢  Let 's initially s et the proble m to it s  
Search
Similar documents
View more...
Tags
Related Search
We Need Your Support
Thank you for visiting our website and your interest in our free products and services. We are nonprofit website to share and download documents. To the running of this website, we need your help to support us.

Thanks to everyone for your continued support.

No, Thanks