I'm a little confused on how to set up the Kalman filter matrices to fuse an ultrasonic altitude sensor and accelerometer from IMU. I am trying to implement this for a quadcopter. And what I'm confused about specifically is when the quadcopter is tilted ($\phi \neq 0, \theta \neq 0$). Do I just simply rotate the z-accelerometer, $a_z$, measurement by $\cos(\phi) \cos(\theta)$? As such:

\begin{equation*} \begin{matrix} \begin{bmatrix} z\\ w \end{bmatrix}_{k+1} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} z \\ w \end{bmatrix}_{k} \end{matrix} + \begin{bmatrix} \frac{1}{2} \Delta t^2\\ \Delta t \end{bmatrix} (a_z \cos\phi \cos\theta) \end{equation*}

Or just keep it as $a_z$?

Also how would my observation matrix H take into the account the roll/pitch angles? Since the ultrasonic is not pointing directly downwards.

I've seen implementations where they do not include the cosines, but I'm not sure if that's because they assume $\cos(x)$ is ~1 for small angles, which would not be valid for my quad (>30deg maneuvers).


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.