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 &= A\sin(\omega t + \phi) + O\\ \dot{x} &= A\omega \cos(\omega t +\phi)\\ \ddot{x} &= -\omega^2 A \sin(\omega t +\phi)\\ \implies \ddot{x} &= -\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 & 1 & 0\\ -\omega^2 & 0 & \omega^2 \\ 0 & 0 & 0 \\ \end{bmatrix} \cdot \begin{bmatrix} x \\ \dot{x} \\ O \end{bmatrix} $$
$\implies$ the system dynamics matrix $\mathbf F$ is:
$$ \begin{bmatrix} 0 & 1 & 0\\ -\omega^2 & 0 & \omega^2 \\ 0 & 0 & 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) & \dfrac{\sin(T_s \omega)}{\omega} & -\cos(T_s \omega) + 1\\ -\omega \sin(T_s \omega) & \cos(T_s \omega) & \omega \sin(T_s \omega) \\ 0 & 0 & 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:
