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:

enter image description here

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:

enter image description here

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.

  • $\begingroup$ Wow, not a degrees-vs-radians thing, is it? $\endgroup$
    – r-bryan
    Feb 1, 2022 at 1:32
  • $\begingroup$ I don't think so, I'll double check but i'm pretty sure everything is supposed to be in degrees. $\endgroup$ Feb 1, 2022 at 1:55
  • $\begingroup$ Did you measure/calibrate the biases of the IMU? $\endgroup$
    – fibonatic
    Jul 21, 2022 at 6:14

1 Answer 1


I wanted to comment but it did not allow me, so I am writing an answer!

If matrix $A$ is Jacobian of $f(x,u)$, then the upper right element of $A$, i.e., A(1,2), should be $(\dot{\theta}\sin(\phi)+\dot{\psi}\cos(\phi))\sec^2(\theta)$.

The sign of cosine is positive.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.