I'm trying to implement an extended Kalman filter to fuse accelerometer and gyroscope data to estimate roll ($\phi$) and pitch ($\theta$). I've found a lot of kalman filter questions but couldn't find one that helped for my specific situation. I've implemented the filter with the below equations and matrices, gotten from the "small unmanned aircraft" book by Beard and McLain.
state predict with new gyroscope data: $$ x_{n + 1} = x_n + T_s f(x, u) \\ P_{n+1} = P_n + T_s (AP_n + P_nA^T + Q) $$
measurement update with new accelerometer data ($y$):
$$ K = PC^T(CPC^T + R)^{-1} \\ x = x + K(y - h(x, u)) \\ P = (I - KC)P $$
f(x, u) =
$$\begin{matrix} \dot \phi + \dot \theta sin(\phi)tan(\theta) + \dot \psi cos(\phi)tan(\theta)\\ \dot \theta cos(\phi) - \dot \psi sin(\phi) \end{matrix}$$
where $\dot \phi, \dot \theta, \dot \psi$ are the roll, pitch, and yaw angular velocity from the gyroscope
A = $$\begin{matrix} (\dot \theta cos(\phi) - \dot \psi sin(\phi))tan(\theta) & (\dot \theta sin(\phi) - \dot \psi cos(\phi))sec^2(\theta)\\ -\dot \theta sin(\phi) - \dot \psi cos(\phi) & 0 \end{matrix}$$
h(x, u) = $$\begin{matrix} gsin(\theta)\\ -gcos(\theta)sin(\phi)\\ -gcos(\theta)cos(\phi) \end{matrix}$$
C =
$$\begin{matrix} 0 & gcos(\theta)\\ -gcos(\phi)cos(\theta) & gsin(\phi)sin(\theta)\\ -gsin(\phi)cos(\theta) & gsin(\theta)cos(\phi) \end{matrix}$$
my state vector is:
x = $$\begin{matrix} \phi\\ \theta \end{matrix}$$ and they're both initially set to 0.
$T_s$ is the time step, and is set to 0.1 since that's how often i call the predict and update functions. The Q matrix is a diagonal of 1.5 and the R matrix is a diagonal of 0.025.
When I actually implement this, i get the following output, which is pretty stable until i move the IMU:
I'm not sure why it would settle with a bias of 2, are there any common pitfalls that cause something like this?
When i actually try to rotate it about x or y, the output goes crazy:
It gets bit better and changes less rapidly when i make Ts smaller, but that doesn't seem like the right solution. Is there another problem I'm not seeing in my math?
If not then I'll go back and look over my actual code. I thought if i include that here too, then the post will get too long, if not I'll edit to add my code as well.
Thanks for the help.
