MixedNoiseSystemModel

The MixedNoiseSystemModel 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) + r_k \enspace,$$ with

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

  • system noise \(w_k \in \mathbb{R}^W \), and

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

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

Usage

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

  • create a new subclass of MixedNoiseSystemModel,

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

  • set the system noise \(w_k\) with the inherited setNoise() method, and

  • set the additive system noise \(r_k\) with the inherited setAdditiveNoise() 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 MixedNoiseSystemModel's properties.

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) + r_k = \begin{bmatrix} p_{k-1} q_{k-1}^2 w_k^2 \\ p_{k-1}^2 + 3 q_{k-1} w_k^3 \end{bmatrix} + r_k \enspace,$$ where \(w_k\) is zero-mean white Gaussian noise with variance \(\sigma^2 = 0.1\) and \(r_k\) zero-mean white Gaussian noise with covariance matrix \(\mathbf{Q} = \mathbf{I}_2\).

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)}]\) and corresponding noise samples \([w_k^{(1)}, \ldots, w_k^{(L)}]\) as it is done in the following. That is, systemEquation() has to compute for each pair of state sample \(x_{k-1}^{(i)}\) and noise sample \(w_k^{(i)}\) the corresponding predicted state \(x_k^{(i)} = a_k(x_{k-1}^{(i)}, w_k^{(i)})\) and return all predicted states as matrix \([x_{k}^{(1)}, \ldots, x_{k}^{(L)}]\).

classdef MixedNoiseSysModelExample < MixedNoiseSystemModel
    methods
        function obj = MixedNoiseSysModelExample()
            % Specify the Gaussian system noise.
            % Of course, you do not have to call setNoise() and setAdditiveNoise() in the constructor.
            % You can also set/change the noise after creating a MixedNoiseSysModelExample object.
            obj.setNoise(Gaussian(0, 0.1));
            obj.setAdditiveNoise(Gaussian(zeros(2, 1), eye(2)));
        end
        
        function predictedStates = systemEquation(obj, stateSamples, noiseSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            w = noiseSamples;
            
            predictedStates = [p .* q.^2 .* w.^2
                               p.^2 + 3*q .* w.^3];
        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}, w_k)\)

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}, w_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 SystemModel 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 and nominal system 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 MixedNoiseSysModelExample < MixedNoiseSystemModel
    methods
        function obj = MixedNoiseSysModelExample()
            % Specify the Gaussian system noise.
            % Of course, you do not have to call setNoise() and setAdditiveNoise() in the constructor.
            % You can also set/change the noise after creating a MixedNoiseSysModelExample object.
            obj.setNoise(Gaussian(0, 0.1));
            obj.setAdditiveNoise(Gaussian(zeros(2, 1), eye(2)));
        end
        
        function predictedStates = systemEquation(obj, stateSamples, noiseSamples)
            p = stateSamples(1, :);
            q = stateSamples(2, :);
            w = noiseSamples;
            
            predictedStates = [p .* q.^2 .* w.^2
                               p.^2 + 3*q .* w.^3];
        end
        
        function [stateJacobian, noiseJacobian, ...
                  stateHessians, noiseHessians] = derivative(obj, nominalState, nominalNoise)
            p = nominalState(1);
            q = nominalState(2);
            w = nominalNoise;
            
            % Jacobians
            stateJacobian = [q^2*w^2 2*p*q*w^2
                               2*p     3*w^3  ];
            
            noiseJacobian = [2*p*q^2*w
                              9*q*w^2 ];
            
            % Hessians
            if nargout >= 3
                % Hessian for p_k
                stateHessians(:, :, 1) = [   0    2*q*w^2
                                          2*q*w^2 2*p*w^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*w;
            end
        end
    end
end

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