9  Example 1 - tracking a random sinusoidal signal

Before introducing more general nonlinear dyanmic systems (see the following examples), let us consider a simple case of tracking a random sine signal whose angular velocity and amplitude vary, randomly, over time. We express the nonlinearity through the measurement model, though this could be done with the dynamic model, as in the examples below.

Let the state vector be

\[ \mathbf{x}_k = \left[\begin{array}{c} \theta_k\\ \omega_k \\ a_k \end{array}\right], \]

where the \(\theta\) is the angle, \(\omega\) the angular velocity and \(a\) the amplitude of the sine function. The model is then

\[\begin{align} \frac{\mathrm{d} \theta}{\mathrm{d}t} &= \omega \\ \frac{\mathrm{d} \omega}{\mathrm{d}t} &= w_{\omega}(t) \\ \frac{\mathrm{d} a}{\mathrm{d}t} &= w_a(t) \end{align}\]

In matrix form, we have

\[ \frac{\mathrm{d}\mathbf{x}}{\mathrm{d}t} = F \mathbf{x}(t) + L \mathbf{w}(t) , \quad \mathbf{x}(0) = \mathbf{x}_0, \]

where

\[ F = \begin{bmatrix} 0 & 1 & 0\\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}, \quad L = \begin{bmatrix} 0 & 0\\ 1 & 0 \\ 0 & 1 \end{bmatrix}, \quad \mathbf{w}(t) = \begin{bmatrix} w_\omega (t)\\ w_a(t) \end{bmatrix}, \]

and the power spectral density of the white noise process \(\mathbf{w}(t)\) is

\[ Q^c = \begin{bmatrix} q_1 & 1\\ 0 & q_2 \end{bmatrix}. \]

Using the matrix exponential formulas (see KF-tutorial), we get the discrete system

\[\begin{align} \mathbf{x}_{k+1 } &= A_k \mathbf{x}_{k} + \mathbf{q}_{k}, \\ y_k &= a_k \sin \theta_k + r_k \end{align}\]

where \(r_k \sim \mathcal{N}(0, \sigma_r)\) is univariate Gaussian noise,

\[ A_k = \begin{bmatrix} 0 & \Delta t & 0\\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}, \]

and \(\mathbf{q}_{k} \sim \mathcal{N}(0, Q_k)\) with

\[ Q_k = \begin{bmatrix} q_1 \Delta t^3 / 3 & q_1 \Delta t^2 / 2 & 0 \\ q_1 \Delta t^2 / 2 & q_1 \Delta t & 0 \\ 0 & 0 & q_2 \Delta t \end{bmatrix}. \]

Finally, the Jacobian of \(h(\mathbf{x}_{k}) = a_k \sin \theta_k\) is easily seen to be

\[ Dh_k = [ a_k \cos \theta_k \quad 0 \quad \sin \theta_k ] \]