I am attempting to create a Kalman filter to track a sine wave (I am using a linear Kalman filter example assuming I already know the frequency of the sine wave) - the example I am using is derived on pages 194-196 of "Fundamentals of Kalman Filtering: A Practical Approach" 2nd edition by Paul Zarchan and Howard Musoff.
It is working to track the AC part of the signal, however the offset of the sine wave from the $x$-axis is not correct, it seems to be tied to my value of $R$, I'm guessing it is because in the derivation of the model no offset is included but I cannot see where an offset term should be inserted, how should I go about this?
The matrices describing my system are like so:
$$\mathbf \Phi_k = \begin{bmatrix} \cos(\omega T_s) & \dfrac{\sin(\omega T_s)}{\omega} \\ -\omega \sin(\omega T_s) & \cos(\omega T_s) \\\end{bmatrix}$$
Where the state matrix is:
$$\mathbf X = \begin{bmatrix} x \\ 0 \end{bmatrix}$$
And I have set $\mathbf Q$ and $\mathbf P$ like so:
$$\mathbf Q = \begin{bmatrix} \dfrac{T_S^3}{3} & \dfrac{T_s^2}{2} \\ \dfrac{T_s^2}{2} & T_s \end{bmatrix}, \quad\mathbf P = \begin{bmatrix} 9999999999999 & 0 \\ 0 & 9999999999999 \end{bmatrix} $$
I am now attemping to use this to track a generated signal of a noisy sine wave of amplitude and frequncy $1$ with an offset of $300$.
If $R$ is set to $0.1$ I get the following output of my filter:
Which is offset by $300/20$, as can be seen from this plot where I add an offset to the Kalman filter output:
Changing $R$ changes this offset (as one might expect as $R$ denotes the uncertainty in your data).
How can I add this offset into my model such that my Kalman filter can tracking the sine wave correctly regardless of offset?
EDIT: I've been thinking more about my question and realised that in a real signal with noise this offset could be considered a separate constant signal with frequency $0$ which could be extracted by using a Kalman filter fitting a constant value (or simply a moving average filter) and that the offset is not inherently part of the sine wave.
- In which case how might I remove the offset in the Kalman filter output entirely? (e.g. if I used a bandpass filter no offset would be present).
- Since my model does not include an offset I might expect no offset at all but I do see an offset of $300/20$, as discussed. Why does this occur if there is no such offset parameter in my model?
Answer
Well, in continuous time, a sinusoid with a bias can be seen as the output of the linear system \begin{align*} \begin{bmatrix}\dot x_1\\\dot x_2\\\dot x_3\end{bmatrix} &= \begin{bmatrix}0 & 1 & 0\\-\omega^2&0&0\\0 &0 &0\end{bmatrix}\begin{bmatrix}x_1\\x_2\\x_3\end{bmatrix}\\ y &= \begin{bmatrix}1&0&1\end{bmatrix} \begin{bmatrix}x_1\\x_2\\x_3\end{bmatrix} \end{align*} As the initial condition $(x_1(0),x_2(0),x_3(0))$ vary in $\mathbb R^3$, the system generates all the possible $y(t)$ of the form $$ y(t) = a\cos(\omega t+\phi) + b $$ The system is observable for all $\omega\ne 0$,therefore a Kalman Filter is possible. With $x:=col(x_1,x_2,x_3)$ and $$ A=\begin{bmatrix}0 & 1 & 0\\-\omega^2&0&0\\0 &0 &0\end{bmatrix},\qquad C=\begin{bmatrix}1&0&1\end{bmatrix} $$ The filter has the form \begin{align*} \dot{\hat x} = A\hat x -L(C\hat x-y) \end{align*} Where $L$ is chosen according to the Kalman theory, on the basis of the covariance matrices of the noises and the solution of the Riccati Equation. For discrete time systems the procedure is the same.
EDIT:-----
If you only use a model for the oscillator you are actually restricting the $A$ map to the $x_1$-$x_2$ subspace. That's equivalent to take $C=[1\;0\;0]$ in the above system. In this latter case you loose observability (actually you do not have neither detectability) and therefore you cannot observe the bias.
No comments:
Post a Comment