Tuning Kalman Filter to Improve State Estimation

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;```

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 anonumous 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```

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```