MeasurementModel

The MeasurementModel 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, v_k)\) and

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

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

Usage

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

  • create a new subclass of MeasurementModel,

  • implement \(h_k(x_k, v_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 MeasurementModel'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 v_k^2 \\ p_k^2 + 3 q_k v_k^3 \end{bmatrix} \enspace,$$ where \(v_k\) is zero-mean white Gaussian noise with variance \(\sigma^2 = 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)}]\) and corresponding noise samples \([v_k^{(1)}, \ldots, v_k^{(L)}]\) as it is done in the following. That is, measurementEquation() has to compute for each pair of state sample \(x_k^{(i)}\) the noise sample \(v_k^{(i)}\) the corresponding measurement \(y_k^{(i)} = h_k(x_k^{(i)}, v_k^{(i)})\) and return all measurements as matrix \([y_k^{(1)}, \ldots, y_k^{(L)}]\).

classdef MeasModelExample < MeasurementModel
    methods
        function obj = MeasModelExample()
            % Specify the Gaussian system noise.
            % Of course, you do not have to call setNoise() in the constructor.
            % You can also set/change the noise after creating a MeasModelExample object.
            obj.setNoise(Gaussian(0, 0.1));
        end
        
        function measurements = measurementEquation(obj, stateSamples, noiseSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            v = noiseSamples;
            
            measurements = [p .* q.^2 .* v.^2
                            p.^2 + 3*q .* v.^3];
        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, v_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, v_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 MeasurementModel 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 and nominal measurement noise, e.g., the prior state mean and the noise mean.

Example

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

classdef MeasModelExample < MeasurementModel
    methods
        function obj = MeasModelExample()
            % Specify the Gaussian system noise.
            % Of course, you do not have to call setNoise() in the constructor.
            % You can also set/change the noise after creating a MeasModelExample object.
            obj.setNoise(Gaussian(0, 0.1));
        end
        
        function measurements = measurementEquation(obj, stateSamples, noiseSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            v = noiseSamples;
            
            measurements = [p .* q.^2 .* v.^2
                            p.^2 + 3*q .* v.^3];
        end
        
        function [stateJacobian, noiseJacobian, ...
                  stateHessians, noiseHessians] = derivative(obj, nominalState, nominalNoise)
            p = nominalState(1);
            q = nominalState(2);
            v = nominalNoise;
            
            % Jacobians
            stateJacobian = [q^2*v^2 2*p*q*v^2
                               2*p     3*v^3  ];
            
            noiseJacobian = [2*p*q^2*v
                              9*q*v^2 ];
            
            % Hessians
            if nargout >= 3
                % Hessian for p_k
                stateHessians(:, :, 1) = [   0    2*q*v^2
                                          2*q*v^2 2*p*v^2];
                
                % Hessian for q_k
                stateHessians(:, :, 2) = [2 0
                                          0 0];
                
                % Hessian for p_k
                noiseHessians(:, :, 1) = 2*p*q^2;
                
                % Hessian for q_k
                noiseHessians(:, :, 2) = 18*q*v;
            end
        end
    end
end

If you're estimator only requires the Jacobians of \(h_k(x_k, v_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.