# How to include phase noise in sinusoidal kalman filter model?

I am modelling simple sinusoidal motion using a Kalman filter from the following equation of motion:

$\ddot{x} = -\omega^2x$

So my matrix equation of motion is:

$$\begin{bmatrix} \dot{x} \\ \ddot{x} \\ \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ -\omega^2 & 0 \\ \end{bmatrix} . \begin{bmatrix} x \\ \dot{x} \\ \end{bmatrix}$$

and my system dynamics matrix was

$$A = \begin{bmatrix} 0 & 1 \\ -\omega^2 & 0 \\ \end{bmatrix}$$

I then performed the transformation $\mathbf{F(t)} = \mathcal{L}^{-1} \left( s \mathbf{I} - \mathbf{A} \right) ^{-1}$ to get the state transition matrix like so:

$$F(t) = \begin{bmatrix} \cos(\omega t) & \dfrac{\sin(\omega t)}{\omega} \\ -\omega \sin(\omega t) & \cos(\omega t) \\ \end{bmatrix}$$

Where I then replaced $t$ with my sample time $T_s$ where $T_s = \dfrac{1}{F_s}$ where $F_s$ was my sample frequency.

I then used this to extract my sinusoidal signal. The signal I am tracking is a sinusoid which experiences stochastic changes in phase. I am able to increase the value multiplying the process noise covariance matrix $\mathbf{Q}$ in order to make the filter react faster to these changes in phase, but that also means it lets through more unwanted noise.

If I apply a Kalman with a high value multiplying the process covariance matrix I get the following:

Where grey is the raw signal, blue is the true signal and red is the Kalman filter output. This reacts quickly to the changes in the phase of the sine wave but lets through much of the rest of the signal and noise as shown in the PSD of the filtered and raw signal below:

Where blue is the PSD of the raw signal and orange of the PSD of the filtered signal. From this I can see that very little noise has been removed.

If I decrease the factor multiplying the process noise covariance matrix such that the confidence in the model is increased then the output is less noisy but takes a long time to react to changes in the phase of the wave, as shown in the plot below:

The PSD of this filtered signal compared to the raw signal is shown below:

My question is how can I include this phase noise in my model such that it reacts quickly to phase changes without letting through large amounts of noise?

• What about an inverse notch filter or a PLL? – Arnfinn Jun 27 '17 at 13:09
• The signal that we are applying this to contains the motion of a physical oscillator, which is what I am modelling as sinusoidal motion, I wish to extract the position and velocity using the kalman along with the uncertainties in these estimates. The idea is to eventually use a more sophisticated model of the motion. – SomeRandomPhysicist Jun 27 '17 at 13:18
• The only solution I have come up with so far is applying the current Kalman filter to predict the position and velocity and then applying a bandpass (inverse-notch) to the estimated position and velocity to isolate that signal. It would be ideal if the Kalman could do both, if such a thing is possible. – SomeRandomPhysicist Jun 27 '17 at 13:40
• The result of performing a filter after the Kalman filter appears to be no different than the filter on it's own, although the Kalman filter does extract the velocity for you. – SomeRandomPhysicist Jun 27 '17 at 21:25
• What does the transfer-function from measurement to the state you wish to learn look like when you compute the steady-state Kalman filter? – Arnfinn Jun 28 '17 at 14:23