Covariance matrix, Q, for a Kalman filter given the stochastic differential equation for the state of the system?

Question

Given that I have a stochastic differential equation describing the motion of my system like so:

$$ \ddot{x}(t) + \Omega_0^2x(t) - C\dfrac{dW(t)}{dt} = 0$$

Where $\Omega_0$ and $C$ are constants.

I then have the following equation in matrix form for my system dynamics

\begin{equation} \label{systemsDynamics} \tag{1} \dot{\vec{X}} = \mathbf{A}\vec{X} + \vec{\omega} = \begin{bmatrix} \dot{x} \\ \ddot{x} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1\\ -\Omega_0^2 & 0 \\ \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \\ \end{bmatrix} + \begin{bmatrix} 0 \\ C \\ \end{bmatrix} \dfrac{dW(t)}{dt} \end{equation}

My system then evolves like so:

$$dx = \dot{x} \cdot dt$$ $$d\dot{x} = -\Omega_0^2x \cdot dt + C \cdot dW$$

From this systems dynamics matrix, $\bf{A}$, and neglecting the stochastic term I can calculate my state transition matrix $\bf{F}$ like so:

$$ \mathbf{F} = \mathcal{L}^{-1}([s \mathbf{I} - \mathbf{A}]^{-1}) $$

This gives me the following:

$$ X_{t} = \mathbf{F} X_{t-1} = \begin{bmatrix} x_t \\ \dot{x}_t \\ \end{bmatrix} = \left[\begin{matrix}\cos{\left (\Omega_0 \Delta t \right )} & \frac{1}{\Omega_0} \sin{\left (\Omega_0 \Delta t \right )}\\- \Omega_0 \sin{\left (\Omega_0 \Delta t \right )} & \cos{\left (\Omega_0 \Delta t \right )}\end{matrix}\right] \begin{bmatrix} x_{t-1} \\ \dot{x}_{t-1} \\ \end{bmatrix} $$

Thus far I have been using the standard discrete $\mathbf{Q}$ matrix for a $2^{nd}$ order polynomial filter

$$ \mathbf{Q} = \sigma_Q^2 \begin{bmatrix} \dfrac{\Delta t^3}{3} & \dfrac{\Delta t^2}{2}\\ \dfrac{\Delta t^2}{2} & \Delta t\\ \end{bmatrix} $$

Where I then tune $\sigma_Q$ to produce the best estimate on simulated data.

Is this the correct way to set the process noise matrix?

If I understand correctly the optimal value of $\sigma_Q^2$ should be the variance of the white process noise $\vec{\omega}$ in equation \ref{systemsDynamics} such that $\mathbf{Q} = \mathbb{E}(\vec{\omega}\vec{\omega}^T)$

Answer 1

As far as I can see your discrete-process noise covariance matrix Qk is wrong (as it's based on a polynomial Kalman filter model) whereas your system dynamics matrix A is based on a sinusoidal Kalman filter model.

You should use the following to find the correct discrete-process noise covariance matrix:

$$ Q_k = \int_{0}^{T_s} \Phi(\tau) Q \Phi(\tau)^T d\tau $$

where $\Phi(\cdot)$ is the fundamental matrix as: $$\Phi(\tau) = \left[\begin{matrix}\cos{\left (\Omega_0 \tau \right )} & \frac{1}{\Omega_0} \sin{\left (\Omega_0 \tau \right )}\\- \Omega_0 \sin{\left (\Omega_0 \tau \right )} & \cos{\left (\Omega_0 \tau \right)}\end{matrix}\right]$$

and $Q=\mathcal{E}\{ww^T\}$ is the continuous process noise matrix, which can be taken as
$$Q = \sigma_q^2\left[\begin{matrix}0 & 0 \\ 0 &1\end{matrix}\right]$$ if the noise added to the last state. You should elaborate on the above integral to get the correct discrete-process noise matrix $Q_k$.

For those not interested in taking the integral, Matlab symbolic toolbox produces the following result for the Qk matrix:

>> syms t w T
>> Phi = [cos(w.*t), sin(w.*t)./w ; -w.*sin(w.*t) , cos(w.*t)];
>> Q = [0 0; 0 1];
>> Qk = int(Phi*Q*(Phi.'),t,[0 T])

Qk =

[ -(sin(2*T*w) - 2*T*w)/(4*w^3),     sin(T*w)^2/(2*w^2)]
[            sin(T*w)^2/(2*w^2), T/2 + sin(2*T*w)/(4*w)]