PreviousNext

A Nonlinear Estimation Problem - Part I

We consider estimating the system state $$x_k = \begin{bmatrix} p^x_k \\ p^y_k \\ \varphi_k \\ s_k \\ \dot{\varphi}_k \end{bmatrix}$$ of a target with 2D position \([p^x_k,\, p^y_k]^\top\), direction \(\varphi_k\), speed \(s_k\) along its direction, and angular velocity \(\dot{\varphi}_k\) at discrete time steps \(k\). Its temporal evolution is modeled with a nonlinear constant velocity/turn rate model according to $$x_k = a(x_{k-1}, w) = \begin{bmatrix} p^x_{k-1} + \cos(\varphi_{k-1} + \Delta t (\dot{\varphi}_{k-1} + w^\dot{\varphi})) \cdot \Delta t (s_{k-1} + w^s) \\ p^y_{k-1} + \sin(\varphi_{k-1} + \Delta t (\dot{\varphi}_{k-1} + w^\dot{\varphi})) \cdot \Delta t (s_{k-1} + w^s) \\ \varphi_{k-1} + \Delta t (\dot{\varphi}_{k-1} + w^\dot{\varphi}) \\ s_{k-1} + w^s \\ \dot{\varphi}_{k-1} + w^\dot{\varphi} \end{bmatrix} \enspace,$$ where \(w = [w^s, w^\dot{\varphi}]^\top\) denotes time-invariant, zero-mean white Gaussian noise with covariance matrix $$\mathbf{Q} = \begin{bmatrix} 10^{-1} & 0 \\ 0 & 10^{-3} \end{bmatrix}$$ and \(\Delta t = 0.1\) the discrete step size. At each time step \(k\), we receive a noisy position measurement in polar coordinates according to $$y_k = h(x_k) + v = \begin{bmatrix} \sqrt{(p^x_{k})^2 + (p^y_{k})^2} \\ \operatorname{atan2}(p^y_{k}, p^x_{k}) \end{bmatrix} + v \enspace,$$ where \(\operatorname{atan2}\) denotes the four quadrant inverse tangent and \(v\) also time-invariant, additive, and zero-mean white Gaussian noise with covariance $$\mathbf{R} = \begin{bmatrix} 10^{-2} & 0 \\ 0 & 10^{-4} \end{bmatrix} \enspace.$$ The initial system state \(x_0\) is assumed to be Gaussian distributed $$x_0 \sim \mathcal{N}(\hat{x}_0, \mathbf{C}_0)$$ with mean $$\hat{x}_0 = [1,\, 1,\, 0,\, 0,\, 0]^\top$$ and covariance matrix $$\mathbf{C}_0 = \operatorname{diag}(10, 10, 10^{-1}, 1, 10^{-1}) \enspace.$$

Here, we want to estimate \(x_k\) based on a Kalman filter, i.e., linear filter. This requires an implementation of the above described system and measurement equations.

As you might imagine, the previously used LinearSystemModel and LinearMeasurementModel classes are not the right models for implementing the considered estimation problem. To implement the linear models, is was sufficient to instantiate a linear model class and set the respective model matrices using the provided class methods. In the nonlinear case however, we have to write a new class. This new class must inherit from an appropriate model interface and implement an abstract class method in order to realize the considered system/measurement equation. Name, arguments, and return value of the abstract method depend on the respective model interface.

Implementing the System Model

For implementing the considered constant velocity/turn rate system model, we have to write a new class that inherits from the SystemModel interface. As a consequence, the abstract method

predictedStates = systemEquation(obj, stateSamples, noiseSamples)

has to be implemented. The first argument refers to the concrete class instance and is required to reference class properties or methods and has to be passed as first argument to any class member function in MATLAB (analogous to the this pointer in C++, which is not explicitly passed, though). The second argument is a set of samples representing the current system state. For example, this can be the samples generated by an unscented Kalman filter, particles from a particle filter, or only a single sample like the state mean in case of an extended Kalman filter. The third argument contains a set of system noise samples, where each noise sample is associated to one of the passed state samples. Each estimator is responsible for providing the necessary (and correct) state and noise samples when performing a state prediction.

In the Nonlinear Estimation Toolbox, a set of samples/particles/noise samples is always passed as a matrix where the samples are arranged column-wise, e.g., a matrix \(S\) containing \(L\) samples \(s_i\) is of the form \(S = [s_1, \ldots, s_L]\). When a method takes state and noise samples, the corresponding samples are stored in the same column. Note that the dimensions of the state and noise samples do not have to be necessarily the same.

The return value predictedStates stores for each state and noise sample a predicted state sample, i.e., the returned matrix has the same number of columns as stateSamples and noiseSamples, and the same number of rows as stateSamples. The following code listing shows the implementation of the system equation \(x_k = a(x_{k-1}, w)\) by writing the class TargetSysModel.

TargetSysModel.m

Implementation of the system equation.
classdef TargetSysModel < SystemModel
    methods
        function obj = TargetSysModel()
            % Set size of discrete time step
            obj.deltaT = 0.1;
            
            % Set time-invariant, zero-mean Gaussian system noise.
            % Note that the system noise can changed at any time by calling
            % the setNoise() method.
            sysNoise = Gaussian(zeros(2, 1), [1e-1 1e-3]);
            
            obj.setNoise(sysNoise);
        end
        
        % Implement the abstract systemEquation() method inherited from SystemModel
        function predictedStates = systemEquation(obj, stateSamples, noiseSamples)
            numSamples = size(stateSamples, 2);
            
            predictedStates = nan(5, numSamples);
            
            px       = stateSamples(1, :);
            py       = stateSamples(2, :);
            dir      = stateSamples(3, :);
            speed    = stateSamples(4, :);
            turnRate = stateSamples(5, :);
            
            speedNoise    = noiseSamples(1, :);
            turnRateNoise = noiseSamples(2, :);
            
            predSpeed    = speed + speedNoise;
            predTurnRate = turnRate + turnRateNoise;
            predDir      = dir + obj.deltaT .* predTurnRate;
            
            predictedStates(1, :) = px + cos(predDir) .* obj.deltaT .* predSpeed;
            predictedStates(2, :) = py + sin(predDir) .* obj.deltaT .* predSpeed;
            predictedStates(3, :) = predDir;
            predictedStates(4, :) = predSpeed;
            predictedStates(5, :) = predTurnRate;
        end
    end
    
    properties (Access = 'private')
        % Stores the step size
        deltaT;
    end
end

The class property deltaT stores the step size \(\Delta t\). Of course, this could also be realized using a variable inside the systemEquation(). However, this allows us to change the \(\Delta t\) easily without touching the class code, e.g., by making it public or providing a set method.

A First Implementation of the Measurement Model

After implementing the system model, let's turn to the measurement model. Here, we can choose between two model interfaces to implement it due to its additive noise characteristic:

  1. MeasurementModel

  2. AdditiveNoiseMeasurementModel

First, we start with the more general MeasurementModel interface. The concept of this interface is deliberately similar to its SystemModel counterpart: we create a new class PolarMeasModelNaive that inherits from MeasurementModel and implement the abstract

measurements = measurementEquation(obj, stateSamples, noiseSamples)

class method. Like for the systemEquation() method, the first argument of measurementEquation() refers to the class instance, the second to the samples/particles from the system state, and the last to the corresponding measurement noise samples. The only difference is that we compute and return a measurement for each state-noise sample pair instead of a predicted state sample. Note that here the obj argument is unused as we do not make any use of a class property or method.

PolarMeasModelNaive.m

First implementation of the polar measurement model.
classdef PolarMeasModelNaive < MeasurementModel
    methods
        function obj = PolarMeasModelNaive()
            % Set time-invariant, zero-mean Gaussian measurement noise.
            % Note that the measurement noise can changed at any time by
            % calling the setNoise() method.
            measNoise = Gaussian(zeros(2, 1), [1e-2 1e-4]);
            
            obj.setNoise(measNoise);
        end
        
        % Implement the abstract measurementEquation() method
        % inherited from MeasurementModel
        function measurements = measurementEquation(obj, stateSamples, noiseSamples)
            px = stateSamples(1, :);
            py = stateSamples(2, :);
            
            measurements = [sqrt(px.^2 + py.^2)
                            atan2(py, px)      ] + noiseSamples;
        end
    end
end

Target Tracking using the Unscented Kalman Filter

Now, we have implemented the required models to do state estimation. Let's test them in combination with a sample-based Kalman filter. Here, with the unscented Kalman filter (UKF).

NonlinearEstimationExample.m

First implementation of the nonlinear state estimation.
function NonlinearEstimationExample()
    % Instantiate system model
    sysModel = TargetSysModel();
    
    % Instantiate measurement Model
    measModel = PolarMeasModelNaive();
    
    % The estimator
    filter = UKF();
    
    % Initial state estimate
    initialState = Gaussian([1 1 0 0 0]', [10, 10, 1e-1, 1, 1e-1]);
    
    filter.setState(initialState);
    
    % Perform a prediction step
    filter.predict(sysModel);
    
    % Show the predicted state estimate
    fprintf('Predicted state estimate:\n\n');
    printStateMeanAndCov(filter);
    
    % Assume we receive the measurement
    measurement = [3 pi/5]';
    
    % Perform a measurement update
    filter.update(measModel, measurement);
    
    % Show the updated state estimate
    fprintf('Updated state estimate:\n\n');
    printStateMeanAndCov(filter);
end

function printStateMeanAndCov(filter)
    [mean, cov] = filter.getStateMeanAndCov();
    
    fprintf('State mean:\n');
    disp(mean);
    
    fprintf('State covariance matrix:\n');
    disp(cov);
    
    fprintf('\n');
end

Note the close resemblance between the first Kalman filter example: setting the initial system state, do a prediction step and a measurement update, and get the system state estimates are identical. Execute the file with

>> NonlinearEstimationExample()
Predicted state estimate:

State mean:
    1.0000
    1.0000
         0
   -0.0000
    0.0000
State covariance matrix:
   10.0110         0         0    0.1100   -0.0000
         0   10.0000         0         0         0
         0         0    0.1010    0.0000    0.0101
    0.1100         0    0.0000    1.1000   -0.0000
   -0.0000         0    0.0101   -0.0000    0.1010

Updated state estimate:

State mean:
    1.3261
    0.5964
   -0.0000
    0.0036
    0.0000
State covariance matrix:
    5.9832    3.7794   -0.0000    0.0657    0.0000
    3.7794    5.9775   -0.0000    0.0415    0.0000
   -0.0000   -0.0000    0.1010   -0.0000    0.0101
    0.0657    0.0415   -0.0000    1.0995    0.0000
    0.0000    0.0000    0.0101    0.0000    0.1010

and you should see the output above. Notice the correlations introduced by the prediction step and the reduced uncertainty by the measurement update.

The predict() and update() methods of a filter return their respective runtimes.

A More Efficient Implementation of the Measurement Model

Up to now, our measurement model implementation relies on a model interface that can handle arbitrary noise terms, i.e., measurement equations \(y_k = h(x_k, v_k)\). The considered model, however, suffers from pure additive noise only, which is very common for modeling noisy measurements. Hence, the specialized model interface AdditiveNoiseMeasurementModel takes this into account.

In contrast to the MeasurementModel interface, the AdditiveNoiseMeasurementModel interface requires to implement the abstract class method

measurements = measurementEquation(obj, stateSamples)

The following PolarMeasModel class implements the measurement model using the AdditiveNoiseMeasurementModel interface.

PolarMeasModel.m

A more efficient implementation of the polar measurement model.
classdef PolarMeasModel < AdditiveNoiseMeasurementModel
    methods
        function obj = PolarMeasModel()
            % Set time-invariant, zero-mean Gaussian measurement noise.
            % Note that the measurement noise can changed at any time by
            % calling the setNoise() method.
            measNoise = Gaussian(zeros(2, 1), [1e-2 1e-4]);
            
            obj.setNoise(measNoise);
        end
        
        % Implement the abstract measurementEquation() method
        % inherited from AdditiveNoiseMeasurementModel
        function measurements = measurementEquation(obj, stateSamples)
            px = stateSamples(1, :);
            py = stateSamples(2, :);
            
            measurements = [sqrt(px.^2 + py.^2)
                            atan2(py, px)      ];
        end
    end
end

It is identical to the PolarMeasModelNaive class except for the missing noiseSamples terms. Nevertheless, setting a measurement noise for a model instance with setNoise() is still required. However, the noise treatment is completely moved into the estimator. That is, the filter is responsible for taking the additive measurement noise correctly into account, not the user. For example, Kalman filters can handle additive noise in a closed-form manner, whereas the ensemble Kalman filter (EnKF) draws for each particle a measurement noise realization that is added to each of the measurements return by measurementEquation().

In order to switch to the new measurement model implementation, we only have to modify a single line in NonlinearEstimationExample.m, that is, we replace the line

measModel = PolarMeasModelNaive();

with

measModel = PolarMeasModel();

That's all! After that re-execute the file with

>> NonlinearEstimationExample()
Predicted state estimate:

State mean:
    1.0000
    1.0000
         0
   -0.0000
    0.0000
State covariance matrix:
   10.0110         0         0    0.1100   -0.0000
         0   10.0000         0         0         0
         0         0    0.1010    0.0000    0.0101
    0.1100         0    0.0000    1.1000   -0.0000
   -0.0000         0    0.0101   -0.0000    0.1010

Updated state estimate:

State mean:
    1.2223
    0.5910
   -0.0000
    0.0024
    0.0000
State covariance matrix:
    5.9180    3.6888   -0.0000    0.0650   -0.0000
    3.6888    5.9126   -0.0000    0.0405   -0.0000
   -0.0000   -0.0000    0.1010   -0.0000    0.0101
    0.0650    0.0405   -0.0000    1.0995   -0.0000
   -0.0000   -0.0000    0.0101   -0.0000    0.1010

and you should notice the changed output for the measurement update. This is due to the fact that the UKF no longer has to sample the measurement noise distribution in order to perform the measurement update (as a consequence, also the samples representing the system state have changed).

Of course, analogous to the AdditiveNoiseMeasurementModel there is a AdditiveNoiseSystemModel for efficiently implementing system models that suffer from pure additive noise, i.e., system models of the form \(x_k = a(x_{k-1}) + w_k\). Like for the AdditiveNoiseMeasurementModel, the filter is responsible for handling the system noise correctly.

Switching to Other Kalman Filters

Not only changing the implemented models is such easy with the Nonlinear Estimation Toolbox. Especially switching between different estimators requires only minimal effort. For example, we want to do the same state estimation with another sample-based Kalman filter, say the Gauss-Hermite Kalman filter (GHKF). All we have to do is to replace the line

filter = UKF();

with

filter = GHKF();

and re-executing the file once again:

>> NonlinearEstimationExample()
Predicted state estimate:

State mean:
    1.0000
    1.0000
   -0.0000
    0.0000
    0.0000
State covariance matrix:
   10.0099         0    0.0000    0.1045    0.0000
         0   10.0011    0.0000   -0.0000    0.0000
    0.0000    0.0000    0.1010   -0.0000    0.0101
    0.1045   -0.0000   -0.0000    1.1000   -0.0000
    0.0000    0.0000    0.0101   -0.0000    0.1010

Updated state estimate:

State mean:
   -6.7017
    1.2411
   -0.0000
   -0.0804
   -0.0000
State covariance matrix:
    1.4642   -1.7086    0.0000    0.0153   -0.0000
   -1.7086    2.3241    0.0000   -0.0178   -0.0000
    0.0000    0.0000    0.1010   -0.0000    0.0101
    0.0153   -0.0178   -0.0000    1.0991   -0.0000
   -0.0000   -0.0000    0.0101   -0.0000    0.1010

Like expected, as the estimator changed also the state estimate changed compared to the UKF results.

Although statistical linearization (done by the sample-based Kalman filters) is, in general, superior to explicit linearization using Taylor series expansion, the extended Kalman filter (EKF) is still frequently used. Thus, once again we replace the estimator by setting the filter variable to

filter = EKF();

in order to use the extended Kalman filter. However, before executing NonlinearEstimationExample(), we set MATLAB's Command Window output display format according to

>> format long

Subsequently, we run

>> NonlinearEstimationExample()
Predicted state estimate:

State mean:
     1
     1
     0
     0
     0
State covariance matrix:
  10.011000000000081                   0                   0   0.110000000000400                   0
                   0  10.000000000000002                   0                   0                   0
                   0                   0   0.101010000000000                   0   0.010100000000000
   0.110000000000400                   0                   0   1.100000000000000                   0
                   0                   0   0.010100000000000                   0   0.101000000000000

Updated state estimate:

State mean:
   2.277277351321657
   1.963124341733931
                   0
   0.014034612790520
                   0
State covariance matrix:
   0.005095005865495   0.004895009754444                   0   0.000055983482690                   0
   0.004895009754444   0.005095005645957                   0   0.000053785942762                   0
                   0                   0   0.101010000000000                   0   0.010100000000000
   0.000055983482690   0.000053785942762                   0   1.098791944679154                   0
                   0                   0   0.010100000000000                   0   0.101000000000000

Thanks to the object-orientated toolbox design, the EKF worked out of the box although it requires the Jacobians of the system and measurement equations!

The toolbox's system model and measurement model interfaces directly come with an approximative derivative of the system/measurement equation based on the finite difference method. Moreover, when closed-form Jacobians and Hessians are possible, you can easily overwrite the approximative finite difference method with the closed-form solutions by overriding the derivative() interface methods.

Implementing Analytic Derivatives

Nonetheless, an explicit implementation of the model Jacobians (and Hessians) can easily be done by overriding the respective derivative() methods of the used system model and measurement model interfaces (here of the SystemModel and AdditiveNoiseMeasurementModel interfaces) according to

TargetSysModel.m

Add closed-form implementation of the system equation Jacobians.
classdef TargetSysModel < SystemModel
    methods
        function obj = TargetSysModel()
            % Set size of discrete time step
            obj.deltaT = 0.1;
            
            % Set time-invariant, zero-mean Gaussian system noise.
            % Note that the system noise can changed at any time by calling
            % the setNoise() method.
            sysNoise = Gaussian(zeros(2, 1), [1e-1 1e-3]);
            
            obj.setNoise(sysNoise);
        end
        
        % Implement the abstract systemEquation() method inherited from SystemModel
        function predictedStates = systemEquation(obj, stateSamples, noiseSamples)
            numSamples = size(stateSamples, 2);
            
            predictedStates = nan(5, numSamples);
            
            px       = stateSamples(1, :);
            py       = stateSamples(2, :);
            dir      = stateSamples(3, :);
            speed    = stateSamples(4, :);
            turnRate = stateSamples(5, :);
            
            speedNoise    = noiseSamples(1, :);
            turnRateNoise = noiseSamples(2, :);
            
            predSpeed    = speed + speedNoise;
            predTurnRate = turnRate + turnRateNoise;
            predDir      = dir + obj.deltaT .* predTurnRate;
            
            predictedStates(1, :) = px + cos(predDir) .* obj.deltaT .* predSpeed;
            predictedStates(2, :) = py + sin(predDir) .* obj.deltaT .* predSpeed;
            predictedStates(3, :) = predDir;
            predictedStates(4, :) = predSpeed;
            predictedStates(5, :) = predTurnRate;
        end
        
        % Override the derviate() method inherited from SystemModel in
        % order to implement analytic derivatives
        function [stateJacobian, ...
                  noiseJacobian] = derivative(obj, nominalState, nominalNoise)
            dir      = nominalState(3);
            speed    = nominalState(4);
            turnRate = nominalState(5);
            
            speedNoise    = nominalNoise(1);
            turnRateNoise = nominalNoise(2);
            
            t = obj.deltaT;
            
            predSpeed    = speed + speedNoise;
            predTurnRate = turnRate + turnRateNoise;
            predDir      = dir + t * predTurnRate;
            
            a =  cos(predDir) * t;
            b =  sin(predDir) * t;
            c = -b * predSpeed;
            d =  a * predSpeed;
            e =  c * t;
            f =  d * t;
            
            stateJacobian = [1 0 c a e
                             0 1 d b f
                             0 0 1 0 t
                             0 0 0 1 0
                             0 0 0 0 1];
            
            noiseJacobian = [a e
                             b f
                             0 t
                             1 0
                             0 1];
        end
    end
    
    properties (Access = 'private')
        % Stores the step size
        deltaT;
    end
end

and

PolarMeasModel.m

Add closed-form implementation of the measurement equation Jacobian.
classdef PolarMeasModel < AdditiveNoiseMeasurementModel
    methods
        function obj = PolarMeasModel()
            % Set time-invariant, zero-mean Gaussian measurement noise.
            % Note that the measurement noise can changed at any time by
            % calling the setNoise() method.
            measNoise = Gaussian(zeros(2, 1), [1e-2 1e-4]);
            
            obj.setNoise(measNoise);
        end
        
        % Implement the abstract measurementEquation() method
        % inherited from AdditiveNoiseMeasurementModel
        function measurements = measurementEquation(obj, stateSamples)
            px = stateSamples(1, :);
            py = stateSamples(2, :);
            
            measurements = [sqrt(px.^2 + py.^2)
                            atan2(py, px)      ];
        end
        
        % Override the derviate() method inherited from
        % AdditiveNoiseMeasurementModel in order to
        % implement the analytic derivative
        function stateJacobian = derivative(obj, nominalState)
            px = nominalState(1);
            py = nominalState(2);
            
            a = px^2 + py^2;
            b = sqrt(a);
            
            stateJacobian = [ px / b, py / b, 0, 0, 0
                             -py / a, px / a, 0, 0, 0];
        end
    end
end

Note that this has no influence on the implemented systemEquation() or measurementEquation() methods.

>> NonlinearEstimationExample()
Predicted state estimate:

State mean:
     1
     1
     0
     0
     0
State covariance matrix:
  10.011000000000001                   0                   0   0.110000000000000                   0
                   0  10.000000000000002                   0                   0                   0
                   0                   0   0.101010000000000                   0   0.010100000000000
   0.110000000000000                   0                   0   1.100000000000000                   0
                   0                   0   0.010100000000000                   0   0.101000000000000

Updated state estimate:

State mean:
   2.277277349629834
   1.963124339261836
                   0
   0.014034612771879
                   0
State covariance matrix:
   0.005095005847403   0.004895009735360                   0   0.000055983482491                   0
   0.004895009735360   0.005095005627874                   0   0.000053785942552                   0
                   0                   0   0.101010000000000                   0   0.010100000000000
   0.000055983482491   0.000053785942552                   0   1.098791944679160                   0
                   0                   0   0.010100000000000                   0   0.101000000000000

As you can see, there is only a slight difference compared to the previous output (hence the above format call; return to the standard output format with format short). Note that the default finite difference derivatives allow you to check your closed-form derivative implementations by comparing your results with the finite difference approximations.

So far we used only linear estimators to do nonlinear state estimation. When nonlinear estimators, e.g., particle filters, are of interest, the implementation of the likelihood function is required. This topic will be covered in the following.

PreviousNext