Kalman Filter, Square Root Information Filter, and Unscented Kalman Filter

 Top  Previous  Next

Below is a numbered list detailing the basic process of performing state estimation using a Kalman Filter, Square Root Information Filter, or Unscented Kalman Filter. For each step in the process, the options available in FreeFlyer script are listed, and a script example is given for each option.

 

There are a variety of Sample Mission Plans (included with your FreeFlyer installation) that demonstrate various applications of these topics. Continue to the Orbit Determination Samples page to view descriptions and images of these examples or jump to one of the Mission Plans listed below.

 

 

For information on creating and configuring a KalmanFilterOD, SquareRootInformationFilterOD, or UnscentedKalmanFilterOD object see Setting up a Kalman Filter, Setting up a Square Root Information Filter, or Setting up an Unscented Kalman Filter, respectively.

 

 

1.Process Observations at the Current Epoch

There are several options available when processing observations.

 

a.Handle observations defined in files
 

i.For tracking data files that have been registered with the Kalman Filter, Square Root Information Filter, or Unscented Kalman Filter, the filter is able to count the number of observations at the current epoch and process them individually in a For loop, as seen in the example below.
 

// Determine the number of observations at this epoch and process each one

n = KalmanFilterOD1.NumberOfObservationsAtCurrentEpoch();
For i = 0 to n-1;
    KalmanFilterOD1.ProcessNextObservation();
End;

 

ii.You can also process data from tracking data files that are not registered with the Kalman Filter, Square Root Information Filter, or Unscented Kalman Filter. The next Observation can be retrieved and then passed to the Filter for processing. For example:
 

// Retrieve an observation from a file and process it

FFGroundObsFile1.GetNextObservation(GroundStationObs1);
KalmanFilterOD1.ProcessObservation(GroundStationObs1);

 

b.Handle observations instantiated and initialized from script
 

i.If you're working with a file format that isn't natively supported by FreeFlyer, you can read the observation data in manually and pass it to the Filter, as seen below:
 

// Assign data that has been read in from a non-supported file format

PointSolutionObs1.X.ObservedValue  = -3410.686122056;

PointSolutionObs1.Y.ObservedValue  =  5950.923165257;

PointSolutionObs1.Z.ObservedValue  = -1788.614088631;

PointSolutionObs1.VX.ObservedValue =     1.892964574;

PointSolutionObs1.VY.ObservedValue =    -1.072030460;

PointSolutionObs1.VZ.ObservedValue =    -7.176379907;

 

PointSolutionObs1.SetObjectBeingObserved(FilterSC);

PointSolutionObs1.SetObserver(FilterSC.GNSSReceivers[0]);
KalmanFilterOD1.ProcessObservation(PointSolutionObs1);

 

ii.You can also process observations that aren't read in from an external file. These observations may be data you're simulating in the current Mission Plan using the SimulateTrackingData method, as shown in the example below.
 

// Simulate and process data

TruthSC.SimulateTrackingData(PointSolutionObs1);

KalmanFilterOD1.ProcessObservation(PointSolutionObs1);

 

2.Evaluate Processing Status

There are a variety of ways that you can evaluate the status of the measurement processing during the run.

 

a.You can determine if measurements were edited by the Filter by examining the measurement ProcessingStatus property:
 

// Report the processing status of this measurement

 

// The Processing status may be:

   - Not Processed

   - Failed Sigma Edit Test

   - Passed Sigma Edit Test

 

Report PointSolutionObs1.X.ProcessingStatus;

 

b.Examine the State Update for each of the properties your filter is estimating:
 

// Report the state updates for a set of position and velocity measurements

Report FilterSC.OD.Cartesian.X.StateUpdate,  FilterSC.OD.Cartesian.Y.StateUpdate,  FilterSC.OD.Cartesian.Z.StateUpdate,

      FilterSC.OD.Cartesian.VX.StateUpdate, FilterSC.OD.Cartesian.VY.StateUpdate, FilterSC.OD.Cartesian.VZ.StateUpdate;

 

c.Inspect Measurement pre- and post-update residuals:
 

// Report the pre- and post-update residuals for a position measurement

Report PointSolutionObs1.X.PreUpdateResidual, PointSolutionObs1.X.PostUpdateResidual;

 

d.Examine the Filter Covariance or Information Matrix:
 

// Report an array containing the elements of the diagonal of the Covariance

Report KalmanFilterOD1.Covariance.Diagonal;

 

// Report a matrix containing all the elements of the Covariance

Report KalmanFilterOD1.Covariance.Matrix;

 

// Report a matrix containing all the elements of the Square Root Information Matrix

SquareRootInformationFilterOD SRIF;

Report SRIF.Covariance.SquareRootInformationMatrix;

 

e. Examine the Kalman Gain:

 

// Report the Kalman Gain

Report KalmanFilterOD1.LastKalmanGain;

 

3.Using the Smoother (optional)

For more information on the available Smoother algorithms for each of the Filters and their configuration options, see Setting up a Smoother.

 

When using the Fixed Point algorithm, the "smoothed state" is updated automatically every time that the Kalman Filter processes an Observation.

 

When using the Fixed Lag or Fixed Lag - Full Span algorithm or the Square Root Information Smoother, since these algorithms are more numerically intensive the "smoothed state" is only updated when the user manually calls the Smoother.CalculateSmoothedState() method. This is to avoid performing the smoother calculations every step, but the user can force those calculations, if desired, by calling the CalculateSmoothedState method each time an Observation is processed at the script level.

 

The Smoother.CalculateSmoothedState() method does NOT need to be called when running in Fixed Point mode, though it is harmless to do so and has no effect.

 

Since the Smoother algorithms run backwards in time, the epoch of the "smoothed state" is always the epoch of the first observation in the Smoother data buffer. The start of the span of the smoother data buffer is determined as the latest epoch of:

 

•The epoch of the first Observation processed by the Kalman filter

•The epoch of the first observation following a gap of data as defined by the "Gap Duration for Reset" property

•The epoch of the first observation following a manual call to the Smoother.Reset() method

•(For Fixed Lag) The epoch of the observation processed n number of points in the past, where n is defined by the "Number of Point in Interval" property.

•(For Fixed Lag - Full Span) The epoch of first observation processed by the Kalman filter.

 

When operating the Kalman Filter using the simple "KalmanFilterOD.StepOD()" method, it isn't directly obvious if the Filter has processed any observations, and hence it isn't automatically known if the "smoothed state" has been updated. To determine this, the user can inspect the Smoother.SmootherStateHasBeenUpdated property. This will return a value of 1 if the smoothed state was updated over the last call to the Kalman Filter, and 0 otherwise.

 

The epoch of the "smoothed state" is accessible through the Smoother.Epoch property. The "smoothed state" itself, including the "smoothed covariance", is accessible through the objects being estimated, as shown below:

 

// Force the calculation of the smoothed state:

// This is applicable when the smoother is configured as a Fixed-Lag or Fixed-Lag - Full Span smoother.

// When in Fixed-Point mode, the smoother updates the smoothed state every time the filter processes an observation.

KalmanFilterOD1.Smoother.CalculateSmoothedState();

 

// Copy the smoothed state into a separate Spacecraft - this enables visualization of differences in views and plots

SmoothedSC.Epoch = KalmanFilterOD1.Smoother.Epoch;

SmoothedSC.X  = EstimatedSC.OD.Cartesian.X.SmoothedValue;

SmoothedSC.Y  = EstimatedSC.OD.Cartesian.Y.SmoothedValue;

SmoothedSC.Z  = EstimatedSC.OD.Cartesian.Z.SmoothedValue;

SmoothedSC.VX = EstimatedSC.OD.Cartesian.VX.SmoothedValue;

SmoothedSC.VY = EstimatedSC.OD.Cartesian.VY.SmoothedValue;

SmoothedSC.VZ = EstimatedSC.OD.Cartesian.VZ.SmoothedValue;

 

// or if using the Equinoctial element set...

 

SmoothedSC.Epoch                = KalmanFilterOD1.Smoother.Epoch;

SmoothedSC.EquinoctialA         = EstimatedSC.OD.Equinoctial.A.SmoothedValue

SmoothedSC.EquinoctialH         = EstimatedSC.OD.Equinoctial.H.SmoothedValue

SmoothedSC.EquinoctialK         = EstimatedSC.OD.Equinoctial.K.SmoothedValue

SmoothedSC.EquinoctialP         = EstimatedSC.OD.Equinoctial.P.SmoothedValue

SmoothedSC.EquinoctialQ         = EstimatedSC.OD.Equinoctial.Q.SmoothedValue

SmoothedSC.EquinoctialLongitude = EstimatedSC.OD.Equinoctial.Longitude.SmoothedValue

 

 

// Propagate to the Smoother Epoch

Step TruthSC to (TruthSC.Epoch == KalmanFilterOD1.Smoother.Epoch);

 

 

// Plot the differences between the truth state and the smoothed state

Plot TruthSC.ElapsedTime, TruthSC.AlongTrackSeparation(SmoothedSC),

    TruthSC.CrossTrackSeparation(SmoothedSC),

    TruthSC.RadialSeparation(SmoothedSC);

 

 

// Look at the smoothed diagonal of the covariance

Report KalmanFilterOD1.Smoother.Epoch.ConvertToCalendarDate(), EstimatedSC.OD.Covariance.SmoothedDiagonal;

 

 

// Map the 3-sigma smoothed Covariance to a Proximity Zone

EstimatedSC.OD.Covariance.MapSmoothedErrorEllipsoid(EstimatedSC.ProximityZones[0], 3);

 

Additionally, incremental smoothed states can optionally be stored in an Ephemeris object for later output or inspection. This includes the smoothed Cartesian state vectors and can optionally include the smoothed covariance, as shown below:

 

// Associate an Ephemeris object with the smoother:

FilteredSC.OD.SetEphemerisForSmoother(/* Ephemeris to populate */ SmoothedEphem,/* Capture covariance? 0 --> No, 1 --> Yes */ 1);

 

// Force the calculation of the smoothed state:

SquareRootInformationFilterOD1.Smoother.CalculateSmoothedState();

 

// Write the ephemeris to file:

Put SmoothedEphem to FFephem "smoothedSolution.ephem";

 

// Load the smoothed states into a separate Spacecraft for analysis:

SmoothedSC.SetPropagatorType(TypeOf(Ephemeris));

(SmoothedSC.Propagator AsType Ephemeris).LoadEphemeris("smoothedSolution.ephem");

 

 

4.Propagate Objects

After processing the observations at the current epoch and generating any desired output in order to inspect the current processing state, you can propagate to future measurements.

 

a.Propagate all OD objects by the Kalman Filter, Square Root Information Filter, or Unscented Kalman Filter Step Size
 

// Step the filter and all associated objects by the filter's step size

// If there are any observations found within the Step, they get processed

 

KalmanFilterOD1.StepOD();

 

b.Propagate all OD objects to a user-specified epoch
 

// Propagate all objects associated with the estimation process to the argument epoch

// If there are any observations found within the Step, they get processed

 

KalmanFilterOD1.StepToEpoch(epoch);

 

c.Propagate all OD objects to the next Epoch of Data from any of the Registered Tracking Data files
 

// Propagate all objects associated with the estimation process to the next observation

 

KalmanFilterOD1.StepToNextObservation();

 

d.Step objects individually
 

// Propagate each object individually, taking care to ensure that the epochs of each object remain synchronized

 

// You can ensure that epochs are synched by:

   - Ensuring each object's step size is the same

   - Using the "Step to" functionality

 

Step GPS;

Step TruthSC;

Step EstimatedSC to (EstimatedSC.Epoch == TruthSC.Epoch);