AdditiveNoiseMeasurementModel

The AdditiveNoiseMeasurementModel interface describes the relationship between a system state \(x_k \in \mathbb{R}^N\) and a noisy measurement \(y_k \in \mathbb{R}^M\) in the form of $$y_k = h_k(x_k) + v_k \enspace,$$ with

  • measurement equation \(h_k(x_k)\) and

  • additive measurement noise \(v_k \in \mathbb{R}^M \).

The following example code can be found in the toolbox's examples.

Usage

In order to write a specific additive noise measurement model, you have to

  • create a new subclass of AdditiveNoiseMeasurementModel,

  • implement \(h_k(x_k)\) by implementing the abstract measurementEquation() method, and

  • set the measurement noise \(v_k\) with the inherited setNoise() method.

The noise has to be an instance of a Distribution subclass, e.g., a Gaussian. See also the list of available Probability Distributions.

You can access the set noise through the AdditiveNoiseMeasurementModel's noise property.

Example

We implement a nonlinear measurement model for a 2D system state \(x_k = [p_k, q_k]^\top\). The measurement model is given by $$y_k = h_k(x_k) + v_k = \begin{bmatrix} p_k q_k^2 \\ p_k^2 + 3 q_k \end{bmatrix} + v_k \enspace,$$ where \(v_k\) is zero-mean white Gaussian noise with covariance matrix \(\mathbf{R} = \operatorname{diag}(0.01, 0.1)\).

The measurementEquation() method has to be implemented such that it can process an arbitrary number of \(L\) passed state samples \([x_k^{(1)}, \ldots, x_k^{(L)}]\) as it is done in the following. That is, measurementEquation() has to compute for each state sample \(x_k^{(i)}\) the corresponding measurement \(y_k^{(i)} = h_k(x_k^{(i)})\) and return all measurements as matrix \([y_k^{(1)}, \ldots, y_k^{(L)}]\).

classdef AddNoiseMeasModelExample < AdditiveNoiseMeasurementModel
    methods
        function obj = AddNoiseMeasModelExample()
            % Specify the Gaussian measurement noise.
            % Of course, you do not have to call setNoise() in the constructor.
            % You can also set/change the noise after creating a AddNoiseMeasModelExample object.
            obj.setNoise(Gaussian(zeros(2, 1), [0.01, 0.1]));
        end
        
        function measurements = measurementEquation(obj, stateSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            
            measurements = [p .* q.^2
                            p.^2 + 3*q];
        end
    end
end

If your measurement model has time-varying components, implement them as class properties and use these inside the measurementEquation(). Before performing a measurement update, you can adapt the measurement model by simply changing the class properties. For example, the measurement noise is handled in this way: by calling the setNoise() method before performing a measurement update, you can implement time-varying measurement noise.

Implement First-Order and Second-Order Derivatives of \(h_k(x_k)\)

If you intend to use an estimator that relies on the first-order and maybe the second-order derivatives of the measurement equation \(h_k(x_k)\), e.g., the EKF or the EKF2, you should consider implementing its analytical derivatives as well.

By default, the derivatives are automatically approximated in the derivative() method inherited from AdditiveNoiseMeasurementModel using the implemented measurementEquation() method and finite differences. However, the approximations might not be accurate enough or computationally expensive compared to an analytic implementation. In such a case, you can simply overwrite the derivative() method, which has to return the first-order and second-order derivatives for a provided nominal system state, e.g., the prior state mean.

Example

We implement the first-order and second-order derivatives for the above example.

classdef AddNoiseMeasModelExample < AdditiveNoiseMeasurementModel
    methods
        function obj = AddNoiseMeasModelExample()
            % Specify the Gaussian measurement noise.
            % Of course, you do not have to call setNoise() in the constructor.
            % You can also set/change the noise after creating a AddNoiseMeasModelExample object.
            obj.setNoise(Gaussian(zeros(2, 1), [0.01, 0.1]));
        end
        
        function measurements = measurementEquation(obj, stateSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            
            measurements = [p .* q.^2
                            p.^2 + 3*q];
        end
        
        function [stateJacobian, stateHessians] = derivative(obj, nominalState)
            p = nominalState(1);
            q = nominalState(2);
            
            % Jacobian
            stateJacobian = [q^2 2*p*q
                             2*p   3  ];
            
            if nargout == 2
                % Hessian for p_k
                stateHessians(:, :, 1) = [ 0  2*q
                                          2*q 2*p];
                
                % Hessian for q_k
                stateHessians(:, :, 2) = [2 0
                                          0 0];
            end
        end
    end
end

If you're estimator only requires the Jacobian of \(h_k(x_k)\), i.e., the EKF, you do not have to implement the second-order derivatives. Simply compute and return solely the Jacobian when executing derivative(). Of course, when switching for example to the EKF2, your system model will not work until you also compute the Hessians.