AdditiveNoiseSystemModel

The AdditiveNoiseSystemModel interface describes the temporal evolution of a system state \(x_k \in \mathbb{R}^N\) in the form of $$x_k = a_k(x_{k-1}) + w_k \enspace,$$ with

  • system equation \(a_k(x_{k-1})\) and

  • additive system noise \(w_k \in \mathbb{R}^N \).

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

Usage

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

  • create a new subclass of AdditiveNoiseSystemModel,

  • implement \(a_k(x_{k-1})\) by implementing the abstract systemEquation() method, and

  • set the system noise \(w_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 AdditiveNoiseSystemModel's noise property.

Example

We implement a nonlinear system model for a 2D system state \(x_k = [p_k, q_k]^\top\). The system model is given by $$x_k = a_k(x_{k-1}) + w_k = \begin{bmatrix} p_{k-1} q_{k-1}^2 \\ p_{k-1}^2 + 3 q_{k-1} \end{bmatrix} + w_k \enspace,$$ where \(w_k\) is zero-mean white Gaussian noise with covariance matrix \(\mathbf{Q} = \operatorname{diag}(0.01, 0.1)\).

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

classdef AddNoiseSysModelExample < AdditiveNoiseSystemModel
    methods
        function obj = AddNoiseSysModelExample()
            % 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 AddNoiseSysModelExample object.
            obj.setNoise(Gaussian(zeros(2, 1), [0.01, 0.1]));
        end
        
        function predictedStates = systemEquation(obj, stateSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            
            predictedStates = [p .* q.^2
                               p.^2 + 3*q];
        end
    end
end

If your system model has time-varying components, implement them as class properties and use these inside the systemEquation(), e.g., a time-varying sampling period \(\Delta t\). Before each state prediction, you can then adapt the system model by simply changing the class properties. For example, the system noise is handled in this way: by calling the setNoise() method before a state prediction, you can implement time-varying system noise.

Implement First-Order and Second-Order Derivatives of \(a_k(x_{k-1})\)

If you intend to use an estimator that relies on the first-order and maybe the second-order derivatives of the system equation \(a_k(x_{k-1})\), 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 AdditiveNoiseSystemModel using the implemented systemEquation() 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 AddNoiseSysModelExample < AdditiveNoiseSystemModel
    methods
        function obj = AddNoiseSysModelExample()
            % 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 AddNoiseSysModelExample object.
            obj.setNoise(Gaussian(zeros(2, 1), [0.01, 0.1]));
        end
        
        function predictedStates = systemEquation(obj, stateSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            
            predictedStates = [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 \(a_k(x_{k-1})\), 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.