How to include phase in a sinusoidal Kalman Filter I start with the equation for sinusoidal motion with an offset and differentiate to get the 2nd order ODE describing the motion of the object.

\begin{align}
x &amp;= A\sin(\omega t + \phi) + O\\
\dot{x} &amp;= A\omega \cos(\omega t +\phi)\\
\ddot{x} &amp;= -\omega^2 A \sin(\omega t +\phi)\\
\implies \ddot{x} &amp;= -\omega^2(x-O) = -\omega^2x + \omega^2O
\end{align}

In matrix form this becomes:

$$ 
\begin{bmatrix}
\dot{x} \\
\ddot{x} \\
\dot{O} 
\end{bmatrix}
=
\begin{bmatrix}
0 &amp; 1 &amp; 0\\
-\omega^2 &amp; 0 &amp; \omega^2 \\
0 &amp; 0 &amp; 0 \\
\end{bmatrix}
\cdot
\begin{bmatrix}
x \\
\dot{x} \\
O
\end{bmatrix}
$$

$\implies$ the system dynamics matrix $\mathbf F$ is:

$$
\begin{bmatrix}
0 &amp; 1 &amp; 0\\
-\omega^2 &amp; 0 &amp; \omega^2 \\
0 &amp; 0 &amp; 0 \\
\end{bmatrix}
$$

then by performing $$
A(t) = \mathscr{\mathbf L}^{-1}\left(s\vec{\mathbf I} - \vec{\mathbf F}\right)^{-1}
$$

I get the following discretised $\mathbf A$ matrix:

$$
\begin{bmatrix}
\cos(T_s \omega) &amp; \dfrac{\sin(T_s \omega)}{\omega} &amp; -\cos(T_s \omega) + 1\\
-\omega \sin(T_s \omega) &amp; \cos(T_s \omega) &amp; \omega \sin(T_s \omega) \\
0 &amp; 0 &amp; 1 \\
\end{bmatrix}
$$

This works to filter a sine wave of a know frequency with an offset, it predicts the correct phase, amplitude and even if a change the amplitude of the signal being filtered half way in it corrects for this very quickly and fits to the true signal. However if I shift the phase of the true signal part way through it does not correct for it. How might I include phase $\phi$ in my Kalman filter model such that it can still track the signal with sudden changes in phase?

If I try with my current filter I get the following result:
The answer is that the filter I detailed can correct for the phase, the issue was that the values in my Q matrix were too small and therefore it was overly trusting of the model and was correcting for the change in phase very slowly. By increasing the values in the Q matrix this model can quickly correct for the change in the phase of the oscillator!

The output now looks like the following: