Tuning Kalman Filter to Improve State Estimation

Since R2022a

This example shows how to tune process noise and measurement noise of a constant velocity Kalman filter.

Motion Model

A Kalman filter estimates the state of a physical object by processing a set of noisy measurements and compares the measurements with a motion model. As an idealized representation of the true motion of the object, the motion model is expressed as a function of time and a set of variables, called the state. The filter usually saves the state in a form of a vector, called a state vector.

This example explores one of the simplest motion models: a constant velocity motion model in two dimensions. A constant velocity motion model assumes that the object moves with a nearly constant velocity. The state vector consists of four parameters that represent the position and velocity in the x- and y- dimensions.

There are other popular motion models, for example a constant turn that assumes the object moves mostly along a circular arc with a constant speed. More complicated models, such as constant acceleration, help when an object moves for a significant duration of time according to the model. Nonetheless, a very simple motion model like the constant velocity model can be used successfully to track an object that gradually changes its direction or speed over time. A Kalman filter achieves this flexibility by providing an additional parameter called process noise.

Process Noise

In reality, objects do not exactly follow a particular motion model. Therefore, when a Kalman filter estimates the motion of an object, it must account for unknown deviations from the motion model. The term ‘process noise’ is used to describe the amount of deviation, or uncertainty, of the true motion of the object from the chosen motion model. Without process noise, a Kalman filter with a constant velocity motion model fits a single straight line to all the measurements. With process noise, a Kalman filter can give newer measurements greater weight than older measurements, allowing for a change in direction or speed. While 'noise' and 'uncertainty' are terms that connote the idea of a chaotic deviation from the model, process noise can also allow for small, predictable, changes to the true motion of an object that would otherwise require considerably more complex motion models.

This example shows a car moving along a curving road with a relatively constant speed profile and uses a constant velocity motion model with a small amount of process noise to account for the minor deviations due to changes in steering and speed. Process noise allows the filter to account for small changes in speed or direction without estimating how fast the car is accelerating or turning. If you examine the trajectory of a car over a short duration of time, you may observe that it goes nearly straight. It is only over larger durations of time that you can observe the change in the direction of motion of the car.

Process noise has an inherent tradeoff. A low process noise may cause the filter to ignore rapid deviations from the true trajectory and instead favor the motion model. This limits the amount of deviation from the motion model that the true trajectory can have at any particular time. A high process noise admits greater local deviations from the motion model but makes the filter too sensitive to noisy measurements.

Measurement Noise

Kalman filters also model "measurement noise" which helps inform the filter how much it should weight the new measurements versus the current motion model. Specifying a high measurement noise indicates that the measurements are inaccurate and causes the filter to favor the existing motion model and react more slowly to deviations from the motion model.

The ratio between the process noise and the measurement noise determines whether the filter follows closer to the motion model, if the process noise is smaller, or closer to the measurements, if the measurement noise is smaller.

Training and Test Trajectories

A simple approach to tune a Kalman filter is to use measurements with a known ground truth and adjusting the measurement and process noise of the filter.

A constant velocity filter tuned to follow an object that has a steady speed and turns very slowly over a long distance, may not work as well when estimating an object that slows down or turns quickly. Therefore, it is important to tune the filter for the entire range of motion types you expect it to filter. It is also important to consider the expected amount of measurement noise. If you tune the filter using low-noise measurements, the filter may track changes in the motion model better. However, if you use the same tuned filter to track an object measured in a higher noise environment, the resulting track may be unduly influenced by outliers.

The following code generates a training trajectory that models the path of a vehicle travelling at 30 m/s on a highway. It also generates a test trajectory for a vehicle that has a similar speed and follows a highway of similar curvature variation.

% Specify the training trajectory
trajectoryTrain = waypointTrajectory( ...
    [96.4 159.2 0; 2047 197 0;2245 -248 0; 2407 -927 0], ...
    [0; 71; 87; 110], ...
    'GroundSpeed', [30; 30; 30; 30], ...
    'SampleRate', 2);

dtTrain = 1/trajectoryTrain.SampleRate;
timeTrain = (0:dtTrain:trajectoryTrain.TimeOfArrival(end));
[posTrain, ~, velTrain] = lookupPose(trajectoryTrain,timeTrain);

% Specify the test trajectory
trajectoryTest = waypointTrajectory( ...
    [-2.3 72 0; -137 -204 0; -572 -937 0; -804 -1053 0; -887 -1349 0; ...
     -674 -1608 0; 368 -1604 0; 730 -1599 0; 1633 -1581 0; 1742 -1586 0], ...
    [0; 8; 34; 42; 53; 64; 97; 107; 133; 136], ...
    'GroundSpeed', [35; 35; 34; 30; 30; 30; 35; 35; 35; 35], ...
    'SampleRate', 2);

dtTest = 1/trajectoryTest.SampleRate;
timeTest = (0:dtTest:trajectoryTest.TimeOfArrival(end));
[posTest, ~, velTest] = lookupPose(trajectoryTest,timeTest);

% Plot the trajectories
figure
plot(posTrain(:,1),posTrain(:,2),'.', ...
     posTest(:,1), posTest(:,2),'.');
axis equal;
grid on;
legend('Training','Test');
xlabel('X Position (m)');
ylabel('Y Position (m)');
title('True Position')

Setting up the Kalman Filter

As stated above, create a Kalman Filter to use a two-dimensional motion model.

KF = trackingKF('MotionModel','2D Constant Velocity')

KF = 
  trackingKF with properties:

               State: [4x1 double]
     StateCovariance: [4x4 double]

         MotionModel: '2D Constant Velocity'
        ProcessNoise: [2x2 double]

    MeasurementModel: [2x4 double]
    MeasurementNoise: [2x2 double]

     MaxNumOOSMSteps: 0

     EnableSmoothing: 0

Conventions

This example contains position-only measurements of the vehicles. The constant velocity motion model, '2D Constant Velocity', has a state vector of the form: [px; vx; py; vy], where px and py are the positions in the x- and y-directions, respectively; and vx and vy are the velocities in the x- and y-directions, respectively.

trueStateTrain = [posTrain(:,1), velTrain(:,1), posTrain(:,2), velTrain(:,2)]';
trueStateTest = [posTest(:,1), velTest(:,1), posTest(:,2), velTest(:,2)]';

Since the truth is known for both the training and test data, you can directly simulate measurements. You obtain the position measurement by pre-multiplying the state vector by the MeasurementModel property of the filter. The MeasurementModel property, specified as the matrix, [1 0 0 0; 0 0 1 0], corresponds to position-only measurements. You also add measurement noise to these position measurements.

s = rng;
rng(2021);
posSelector = KF.MeasurementModel; % Position from state
rmsSensorNoise = 5; % RMS deviation of sensor data noise [m]

% Training Data - Normal Sensor. Position Only
truePosTrain = posSelector * trueStateTrain;
measPosTrain = truePosTrain + rmsSensorNoise * randn(size(truePosTrain))/sqrt(2);

% Test Data - Normal Sensor. Position Only
truePosTest = posSelector * trueStateTest;
measPosTest = truePosTest + rmsSensorNoise * randn(size(truePosTest))/sqrt(2);

Now the filter can be constructed and tuned using the training data and evaluated using the test data.

Choosing Initial Conditions

A simple way to initialize the state vector is to use the first position measurement and approximate the velocity using the first two measurements.

initStateTrain([1 3]) = measPosTrain(:,1);
initStateTrain([2 4]) = (measPosTrain(:,2) - measPosTrain(:,1))./dtTrain;
initStateTest([1 3]) = measPosTest(:,1);
initStateTest([2 4]) = (measPosTest(:,2) - measPosTest(:,1))./dtTest;

The state covariance matrix should also be initialized to account for the uncertainty in measurement. To start, initialize just the main diagonal via$\mathrm{diag}\left[\begin{array}{cccc}{\sigma }_{\mathrm{Px}}& {\sigma }_{\mathrm{Vx}}& {\sigma }_{\mathrm{Py}}& {\sigma }_{\mathrm{Vz}}\end{array}\right]$and adjust the position terms to correspond to the noise of the sensor. The velocity terms have higher noise since they are based on two measurements of position, not one.

initStateCov = diag(([1 2 1 2]*rmsSensorNoise.^2));

Choosing Process Noise

Process noise can be estimated via the expected deviation from constant velocity using a mean squared step change in velocity at each time step. Using the scalar form for process noise ensures that the components in the x- or y- directions are treated equally.

Qinit = var(vecnorm(diff(velTrain)./dtTrain,2,1))

Qinit = 
15.2404

For many sensors, measurement noise is a known quantity often measured with an RMS value. Initialize the covariance to the square of this value.

Rinit = rmsSensorNoise.^2

Rinit = 
25

Now you can set the process noise and measurement noise on the filter object.

KF.ProcessNoise = Qinit;
KF.MeasurementNoise = Rinit;

Initial Results

Training Set

The helper function, evaluateFilter sets up the filter and computes the RMS error of the predicted position against the true position of the training set:

errorTunedTrain = evaluateFilter(KF, initStateTrain, initStateCov, posSelector, dtTrain, measPosTrain, truePosTrain)

errorTunedTrain = 
2.9624

Compare the filtered position error against the uncorrected raw measurements of the training set:

rms(vecnorm(measPosTrain - truePosTrain,2,1))

ans = 
4.7319

It is clear that the filter obtained a much better position estimate.

Test Set

In the test trajectory, the vehicle turns a little bit more and has a few acceleration changes. Therefore, it is expected that the filter estimate may not be as good as that of the training trajectory.

errorTunedTest = evaluateFilter(KF, initStateTest, initStateCov, posSelector, dtTest, measPosTest, truePosTest)

errorTunedTest = 
3.7157

Nevertheless, it still is better than just using the raw measurements alone.

rms(vecnorm(measPosTest - truePosTest,2,1))

ans = 
5.2553

Tuning the Filter

You can sweep through a few process noise values and determine a desirable value for both the training and test cases.

nSweep = 100;
Qsweep = linspace(1,50,nSweep);
errorTunedTrain = zeros(1,nSweep);
errorTunedTest = zeros(1,nSweep);
for i = 1:nSweep
    KF.ProcessNoise = Qsweep(i);
    errorTunedTrain(i) = evaluateFilter(KF, initStateTrain, initStateCov, posSelector, dtTrain, measPosTrain, truePosTrain);
    errorTunedTest(i) = evaluateFilter(KF, initStateTest, initStateCov, posSelector, dtTest, measPosTest, truePosTest);
end
plot(Qsweep,errorTunedTrain,'-',Qsweep,errorTunedTest,'-');
legend("training","test")
xlabel("Process Noise (Q)")
ylabel("RMS position error");

As seen in the above plot and in the code below, the training set has a local minimum when setting Q to 5.4 for the training set, and 13 for the test set.

[minErrorTunedTrain, iMinTrain] = min(errorTunedTrain);
[minErrorTunedTest, iMinTest] = min(errorTunedTest);
minErrorTunedTrain

minErrorTunedTrain = 
2.8540

Qsweep(iMinTrain)

ans = 
5.4545

minErrorTunedTest

minErrorTunedTest = 
3.7141

Qsweep(iMinTest)

ans = 
13.8687

Variations in the optimal point are expected and are due to the differences in the trajectories. With a small amount of speed differences and more turns, the test set is expected to have a larger predicted error. A common way to mitigate differences between training and test data is to use a Monte Carlo simulation involving many training trajectories like the test trajectories.

Automated Tuning

Sometimes measurement parameters such as measurement noise may not be known. In some cases, it is helpful to consider the problem as an optimization problem where you seek to minimize the RMS distance error of the training set over the set of input parameters Q and R.

If the measurement noise is unknown, you can initialize it by comparing the measurements against the true states.

n = length(timeTrain);
measErr = measPosTrain - posSelector * trueStateTrain;
sumR = norm(measErr);
Rinit = sum(vecnorm(measErr,2).^2)./(n+1)

Rinit = 
22.2895

After initializing the measurement noise, you then use an optimization solver to perform the tuning.

In this case, use the fminunc function, which finds a local minimum of a function of an unconstrained parameter vector.

Parameter Vector Construction

Since fminunc works by iteratively changing a parameter vector, you use the constructParameterVector helper function to map the process and measurement covariances into a single vector. See the Supporting Functions section in the end for more details.

initialParams = constructParameterVector(Qinit, Rinit);

Optimization Function Construction

To run the optimization solver, construct the function that evaluates the cost based on these parameters. Create a function, measureRMSError, which runs the EKF and evaluates the root mean squared error of the filtered state against the ground truth. The function uses the noise parameters, initial conditions, measured positions, and ground truth as inputs. For more information, see the Supporting Functions section.

Since the fminunc function requires a function that just uses a single parameter vector, the minimization function is wrapped inside an anonymous function that captures the EKF, the training measurement, the truth data, and other variables needed to run the tracker:

func = @(noiseParams) measureRMSError(noiseParams, KF, initStateTrain, initStateCov, posSelector, dtTrain, measPosTrain, truePosTrain);

Finding the Parameters

Now that all the arguments to fminunc are properly initialized, you can find the optimal parameters.

optimalParams = fminunc(func,initialParams);

Local minimum possible.

fminunc stopped because it cannot decrease the objective function
along the current search direction.

Deconstructing the Parameter Vector

Since the two parameters are in a vector form, convert them to the process noise and measurement noise covariance matrices using the extractNoiseParameters function:

[QautoTuned,RautoTuned] = extractNoiseParameters(optimalParams)

QautoTuned = 
0.5683

RautoTuned = 
2.5158

Notice that the two values differ significantly from their "true" values. This is because it really is the ratio between Q and R that matters, not their actual values.

Evaluating Results

Now that you have the optimized covariance matrices that minimize the residual prediction error, you can initialize the EKF with them and evaluate the results in the same manner as before:

KF.ProcessNoise = QautoTuned;
KF.MeasurementNoise = RautoTuned;
autoTunedRMSErrorTrain = evaluateFilter(KF, initStateTrain, initStateCov, posSelector, dtTrain, measPosTrain, truePosTrain)

autoTunedRMSErrorTrain = 
2.8525

autoTunedRMSErrorTest = evaluateFilter(KF, initStateTest, initStateCov, posSelector, dtTest, measPosTest, truePosTest)

autoTunedRMSErrorTest = 
3.9313

Summary

In this example, you learned how to tune process noise and measurement noise of a constant velocity Kalman filter using ground truth and noisy measurements. You also learned how to use the optimization solver, fminunc, to find optimal values for the process and measurement noise parameters.

Supporting Functions

Evaluating the Kalman Filter

The evaluateFilter function evaluates the distance and Euclidean error of a Kalman filter with the given initial conditions, measurements, and ground truth data. The root-mean-square Euclidean error measures how far away the typical measurement is from the training data.

function tunedRMSE = evaluateFilter(KF, initState, initStateCov, posSelector, dt, measPos, truePos)
initialize(KF, initState, initStateCov);
estPosTuned = zeros(2,size(measPos,2));
magPosError = zeros(1,size(measPos,2));

for i = 2:size(measPos,2)
    predict(KF, dt);
    x = correct(KF, measPos(:,i));
    estPosTuned(:,i) = posSelector * x(:);
    magPosError(i) = norm(estPosTuned(:,i) - truePos(:,i));
end
tunedRMSE = rms(magPosError(10:end));
end

Minimization Functions

Parameter Vector to Noise Matrices Conversions

The constructParameterVector function converts the noise covariances into a two-element column vector, where the first element of the vector is the square root of the process noise and the second element is the square root of the measurement noise.

function vector = constructParameterVector(Q,R)
vector = sqrt([Q;R]);
end

The constructParameterMatrices function converts the two-element parameter vector, v, back to the covariance matrices. The first element is used to construct the process noise. The second element is used to construct the measurement noise. Squaring these numbers ensures that the noise values are always positive.

function [Q,R] = extractNoiseParameters(vector)
Q = vector(1).^2;
R = vector(2).^2;
end

Minimizing Residual Prediction Error

The measureRMSError function takes the noise parameters, the initial conditions, the measured positions, runs the Kalman filter and evaluates the root mean squared error.

function rmse = measureRMSError(noiseParams, KF, initState, initCov, posSelector, dt, measPos, truePos)
    [Qtest, Rtest] = extractNoiseParameters(noiseParams);
    
    KF.ProcessNoise = Qtest;
    KF.MeasurementNoise = Rtest;

    rmse = evaluateFilter(KF, initState, initCov, posSelector, dt, measPos, truePos);
end