# 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 &= 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: • What output are you considering? The phase is defined by the initial conditions, therefore check whether the signal you are considering solves that equation. If yes, then probably you are doing some mistake. I suggest you to take out the influence of the bias from the sinusoid equation and to change your output to match the desired signal Feb 4 '17 at 16:58
• The output I am considering is the sum of x and O. The signal I am attempting to track is a harmonic oscillator (i.e. sinusoidal motion) with random phase changes due to environmental effects. Thinking about it furthur, in the derivation of $\ddot{x}$ I have treated $\phi$ as constant, rather than time varying, so I may need to change the equations. The phase should be constant with random near-instant changes occurring occasionally. I want it to be able to adjust the phase of the model if it detects a change in the input phase in the same way it can adjust the amplitude if that changes. Feb 4 '17 at 17:07
• It should be able to do it. I do not see any difference between phase and amplitude. Try with A=[0 1 0;-w2 0 0;0 0 0] and C=[1 0 1] Feb 4 '17 at 17:10
• It can do it, both with my original A matrix and with your A matrix. The issue was simply that my Q matrix was too small and as such it trusted the model too much and took far too long to update when a change occured. I realised it when I tested your model and my own by changing the amplitude but not the phase (as I have done before in testing) and it didn't correct for it anymore! Thanks very much for helping my diagnose this issue! Feb 4 '17 at 17:31 