Run a Set of Filters
It was already demonstrated that a concrete system or measurement model can be handled by various (partly much different) estimators. For example, the AdditiveNoiseMeasurementModel can be used for the measurement update by both linear and nonlinear estimators.
Now, assume you are interested in the estimation quality of various filters for a given estimation problem. You could do this by instantiating all filters, each with its own variable, and then call setState(), predict(), and update() individually for each filter. This, however, is not only a cumbersome and error-prone approach, it also prohibits easy addition or removal of filters from the evaluation.
In such a case, better use the FilterSet class. It allows you to perform estimation evaluations comprising several filters in a comfortable way. The FilterSet provides for example the setStates(), predict(), update(), or getStatesMeanAndCov() methods to work with the filters. This is illustrated in the next example. Note that each filter in a FilterSet requires a unique name as the filters in a set can easily be accessed by their names.
You can change the default filter name by passing a name to the constructor during class instantiation.
FilterSetExample.m
function FilterSetExample()
% Instantiate system model
sysModel = TargetSysModel();
% Instantiate measurement Model
measModel = PolarMeasModel();
% Setup the filters
filters = FilterSet();
filter = GHKF('GHKF');
filters.add(filter);
% Use the filter name to get a filter instance
filters.get('GHKF').setNumQuadraturePoints(4);
filter = EKF();
filters.add(filter);
filter = EKF('Iterative EKF');
filter.setMaxNumIterations(10);
filters.add(filter);
filter = SIRPF();
filter.setNumParticles(10^5);
filters.add(filter);
% Initial state estimate
initialState = Gaussian([1 1 0 0 0]', [10, 10, 1e-1, 1, 1e-1]);
filters.setStates(initialState);
% Just for the heck of it:
for i = 1:filters.getNumFilters()
% Additionally, you can access each filter by its index
% Note: the filters are stored in alphabetical order, e.g.,
% the GHKF was added before the EKF.
filter = filters.get(i);
name = filter.getName();
state = filter.getState();
fprintf('* The "%s" estimates the system state as a %s distribution.\n', name, class(state));
if Checks.isClass(state, 'Gaussian')
[mean, cov] = state.getMeanAndCov();
fprintf(' Mean and covariance:\n');
disp([mean cov]);
elseif Checks.isClass(state, 'DiracMixture')
fprintf(' Number of mixture components (samples): %d\n', state.getNumComponents());
[mean, cov] = state.getMeanAndCov();
fprintf(' Mean and covariance:\n');
disp([mean cov]);
else
fprintf(' Unexpected distribution\n');
end
end
% Perform a prediction step
filters.predict(sysModel);
% Show the predicted state estimates
fprintf('\n\nPredicted state estimates:\n');
printStateMeansAndCovs(filters);
% Assume we receive the measurement
measurement = [3 pi/5]';
% Perform a measurement update
filters.update(measModel, measurement)
% Show the updated state estimates
fprintf('\n\nUpdated state estimates:\n');
printStateMeansAndCovs(filters);
end
function printStateMeansAndCovs(filters)
numFilters = filters.getNumFilters();
names = filters.getNames();
[stateMeans, stateCovs] = filters.getStatesMeanAndCov();
for i = 1:numFilters
fprintf('\nFilter: %s\n', names{i});
fprintf('State mean and covariance:\n');
disp([stateMeans(:, i) stateCovs(:, :, i)]);
end
endExecuting the file with
>> FilterSetExample() * The "EKF" estimates the system state as a Gaussian distribution. Mean and covariance: 1.0000 10.0000 0 0 0 0 1.0000 0 10.0000 0 0 0 0 0 0 0.1000 0 0 0 0 0 0 1.0000 0 0 0 0 0 0 0.1000 * The "GHKF" estimates the system state as a Gaussian distribution. Mean and covariance: 1.0000 10.0000 0 0 0 0 1.0000 0 10.0000 0 0 0 0 0 0 0.1000 0 0 0 0 0 0 1.0000 0 0 0 0 0 0 0.1000 * The "Iterative EKF" estimates the system state as a Gaussian distribution. Mean and covariance: 1.0000 10.0000 0 0 0 0 1.0000 0 10.0000 0 0 0 0 0 0 0.1000 0 0 0 0 0 0 1.0000 0 0 0 0 0 0 0.1000 * The "SIRPF" estimates the system state as a DiracMixture distribution. Number of mixture components (samples): 100000 Mean and covariance: 1.0066 10.0077 -0.0216 0.0044 0.0031 -0.0041 0.9934 -0.0216 9.9970 -0.0007 -0.0157 -0.0040 -0.0008 0.0044 -0.0007 0.1000 -0.0009 0.0007 0.0000 0.0031 -0.0157 -0.0009 0.9953 0.0015 0.0010 -0.0041 -0.0040 0.0007 0.0015 0.0999 Predicted state estimates: Filter: EKF State mean and covariance: 1.0000 10.0110 0 0 0.1100 0 1.0000 0 10.0000 0 0 0 0 0 0 0.1010 0 0.0101 0 0.1100 0 0 1.1000 0 0 0 0 0.0101 0 0.1010 Filter: GHKF State mean and covariance: 1.0000 10.0100 0.0000 -0.0000 0.1046 -0.0000 1.0000 0.0000 10.0010 -0.0000 0.0000 0.0000 0.0000 -0.0000 -0.0000 0.1010 0.0000 0.0101 0.0000 0.1046 0.0000 0.0000 1.1000 -0.0000 0.0000 -0.0000 0.0000 0.0101 -0.0000 0.1010 Filter: Iterative EKF State mean and covariance: 1.0000 10.0110 0 0 0.1100 0 1.0000 0 10.0000 0 0 0 0 0 0 0.1010 0 0.0101 0 0.1100 0 0 1.1000 0 0 0 0 0.0101 0 0.1010 Filter: SIRPF State mean and covariance: 1.0065 10.0171 -0.0227 0.0039 0.1006 -0.0036 0.9933 -0.0227 9.9977 -0.0011 -0.0187 -0.0038 -0.0006 0.0039 -0.0011 0.1012 -0.0007 0.0108 -0.0006 0.1006 -0.0187 -0.0007 1.0927 0.0021 0.0013 -0.0036 -0.0038 0.0108 0.0021 0.1010 Updated state estimates: Filter: EKF State mean and covariance: 2.2773 0.0051 0.0049 0 0.0001 0 1.9631 0.0049 0.0051 0 0.0001 0 0 0 0 0.1010 0 0.0101 0.0140 0.0001 0.0001 0 1.0988 0 0 0 0 0.0101 0 0.1010 Filter: GHKF State mean and covariance: 0.2957 8.9399 -0.8627 -0.0000 0.0934 -0.0000 1.8188 -0.8627 4.0870 -0.0000 -0.0090 0.0000 0.0000 -0.0000 -0.0000 0.1010 0.0000 0.0101 -0.0074 0.0934 -0.0090 0.0000 1.0999 -0.0000 0.0000 -0.0000 0.0000 0.0101 -0.0000 0.1010 Filter: Iterative EKF State mean and covariance: 2.4257 0.0068 0.0043 0 0.0001 0 1.7624 0.0043 0.0040 0 0.0000 0 0 0 0 0.1010 0 0.0101 0.0157 0.0001 0.0000 0 1.0988 0 0 0 0 0.0101 0 0.1010 Filter: SIRPF State mean and covariance: 2.4106 0.0075 0.0046 0.0075 0.0028 -0.0003 1.7511 0.0046 0.0042 0.0027 -0.0003 -0.0002 -0.1267 0.0075 0.0027 0.1047 0.0317 0.0080 0.1299 0.0028 -0.0003 0.0317 0.8884 0.0300 0.0273 -0.0003 -0.0002 0.0080 0.0300 0.0773
By simply commenting out the lines
filter = EKF(); filters.add(filter);
the EKF can be removed from the evaluation for example. In this fashion, other estimators can be added to the simulation as well.
Filters in a FilterSet are stored in alphabetical order. As a consequence, adding or removing filters from a FilterSet can change the indices of the filters in the set!
After discussing system and measurement models, various linear and nonlinear estimators, system state and measurement generation, and the concept of a filter set, we finally combine all these aspects to a complete simulation of a nonlinear estimation problem.