本文主要是介绍mpu6050姿态解算与卡尔曼滤波（5）可应用于51单片机的卡尔曼滤波器，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
博客4中给出的滤波器状态维数为4维，测量量为3维，每次滤波需要做不少矩阵乘法和求逆运算。如果想在51单片机上实现，计算耗时会比较长。考虑应用场合可以对滤波器适当做一些简化，计算量会大大减小。
首先，陀螺和加速度计融合只能保证俯仰和滚转角收敛，而且从测量方程来看，加速度计的测量只与俯仰和滚转角有关。因此可以考虑滤波器状态选取为俯仰角 θ \theta θ和滚转角 γ \gamma γ，这样状态维数会减少。选取状态为俯仰角和滚转角带来的问题是状态方程必须使用欧拉角微分方程，而欧拉角微分方程中包含三角函数计算，反而增加了计算量。
[ ψ ˙ θ ˙ γ ˙ ] = 1 cos θ [ − sin γ 0 cos γ cos γ cos θ 0 sin γ cos θ sin θ sin γ cos θ − sin θ cos γ ] [ ω n b x b ω n b y b ω n b z b ] \begin{bmatrix}\dot\psi\\\dot\theta\\\dot\gamma\end{bmatrix}=\frac{1}{\cos\theta}\begin{bmatrix}-\sin\gamma&0&\cos\gamma\\ \cos\gamma\cos\theta&0&\sin\gamma\cos\theta\\ \sin\theta\sin\gamma&\cos\theta&-\sin\theta\cos\gamma\end{bmatrix}\begin{bmatrix}\omega_{nbx}^b\\\omega_{nby}^b\\\omega_{nbz}^b\end{bmatrix} ψ˙θ˙γ˙ =cosθ1 −sinγcosγcosθsinθsinγ00cosθcosγsinγcosθ−sinθcosγ ωnbxbωnbybωnbzb
这种情况下考虑滤波器的应用场合。如果在滤波器运行过程中能保证 θ \theta θ和 γ \gamma γ基本保持在0±5°附近，那么欧拉角微分方程可以简化为
[ ψ ˙ θ ˙ γ ˙ ] = [ − γ 0 1 1 0 γ θ γ 1 − θ ] [ ω n b x b ω n b y b ω n b z b ] \begin{bmatrix}\dot\psi\\\dot\theta\\\dot\gamma\end{bmatrix}=\begin{bmatrix}-\gamma&0&1\\ 1&0&\gamma\\ \theta\gamma&1&-\theta\end{bmatrix}\begin{bmatrix}\omega_{nbx}^b\\\omega_{nby}^b\\\omega_{nbz}^b\end{bmatrix} ψ˙θ˙γ˙ = −γ1θγ0011γ−θ ωnbxbωnbybωnbzb
这样状态方程就变得十分简单，可以只取 θ \theta θ和 γ \gamma γ。对于 θ \theta θ和 γ \gamma γ保持在0±5°附近的假设在一些场合下是适用的，比如平衡车、自动保持水平模式的四轴，当然前提是控制可以闭环。
对于测量方程，也可简化为
[ a x a y a z ] = [ − g γ g θ g ] \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}=\begin{bmatrix}-g\gamma \\g\theta\\g\end{bmatrix} axayaz = −gγgθg
测量量只取 a x a_x ax和 a y a_y ay。
这样一来状态和测量方程都变得十分简单，计算量也大大减小，应用在51这样的单片机上也能在10ms内完成姿态解算。
这篇关于mpu6050姿态解算与卡尔曼滤波（5）可应用于51单片机的卡尔曼滤波器的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
