I'm currently studying the Extended Kalman Filter. One problem I've decided to explore is to estimate properties of an unchanging sine wave, provided samples along the curve. This problem seems well suited to the EKF's accommodation of nonlinear systems.
The sine wave is represented as:
$a \cdot sin( d + w \cdot t )$
So I have made this be the estimated state:
$x = \begin{bmatrix} a \\ w \\ d \\ t \end{bmatrix}$
Measurements are provided as samples with value (height of the curve) s at time t:
$z = \begin{bmatrix} s \\ t \end{bmatrix}$
My solution is:
$\begin{bmatrix} a_k \\ w_k \\ d_k \\ t_k \end{bmatrix} = \begin{bmatrix} a_{k-1} \\ w_{k-1} \\ d_{k-1} \\ t_{k-1} + dt \end{bmatrix}$
$\begin{bmatrix} s_k \\ t_k \end{bmatrix} = \begin{bmatrix} a_k \cdot sin( d_k + w_k \cdot t_k ) \\ t_k \end{bmatrix}$
Where the jacobians are:
$F = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
$H = \begin{bmatrix} sin(d + w \cdot t) & a \cdot t \cdot cos(d + w \cdot t) & a \cdot cos(d + w \cdot t) & w \cdot cos(d + w \cdot t) \\ 0 & 0 & 0 & 1 \end{bmatrix}$
I've written a simulation of this problem here with the following parameters:
$s(t) = 10 \cdot sin(\frac{2\pi}{166} \cdot t)$
$R = \begin{bmatrix} 10 & 0 \\ 0 & 10^{-12} \end{bmatrix}$
$Q = \begin{bmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
$xinitial = \begin{bmatrix} 5 \\ 1 \\ 0 \\ 0 \end{bmatrix}$
$Pinitial = \begin{bmatrix} 999 & 0 & 0 & 0 \\ 0 & 999 & 0 & 0 \\ 0 & 0 & 999 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}$
This works pretty well about 80% of the time:
But sometimes it fails spectacularly:
The only difference between each run is that the measurements are resampled.
I've tried tweaking the process noise without success (it is zero above because the model precisely represents the simulated process). What's causing this destabilization? And how can I make the filter resilient to it?
Unfortunately, the EKF has no guarantee of stability. There are some papers which specify the conditions under which the EKF is stable, and the two biggest ones are: (a) The noise on the sensor is not too huge, and (b) the initial conditions are not too far off from the truth. The "too huge" and "too far off" depend on the nonlinearity of the system.
Here are some things to try:
1) Try to start it off with the correct initial conditions and see how much noise it can tolerate.
2) Fix the noise, and see how much of an initial condition it can tolerate.
3) Make the filter less aggressive: meaning reduce the initial P. I would make P positive definite by the way.