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 ]

=\frac{1}{\cos\theta} ​ψ˙​θ˙γ˙​​ ​=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 ]

[ψ˙θ˙γ˙]" role="presentation" style="position: relative;">⎡⎣⎢ψ˙θ˙γ˙⎤⎦⎥$$\left[\begin{array}{c}\dot{\psi }\\ \dot{\theta }\\ \dot{\gamma }\end{array}\right]$$

=

[−γ0110γθγ1−θ]" role="presentation" style="position: relative;">⎡⎣⎢−γ1θγ0011γ−θ⎤⎦⎥$$\left[\begin{array}{ccc}-\gamma & 0& 1\\ 1& 0& \gamma \\ \theta \gamma & 1& -\theta \end{array}\right]$$

[ωnbxbωnbybωnbzb]" role="presentation" style="position: relative;">⎡⎣⎢⎢ωbnbxωbnbyωbnbz⎤⎦⎥⎥$$\left[\begin{array}{c}{\omega }_{nbx}^b\\ {\omega }_{nby}^b\\ {\omega }_{nbz}^b\end{array}\right]$$

​ψ˙​θ˙γ˙​​ ​= ​−γ1θγ​001​1γ−θ​ ​ ​ωnbxb​ωnbyb​ωnbzb​​ ​
这样状态方程就变得十分简单，可以只取$\theta$和$\gamma$。对于$\theta$和$\gamma$保持在0±5°附近的假设在一些场合下是适用的，比如平衡车、自动保持水平模式的四轴，当然前提是控制可以闭环。
对于测量方程，也可简化为
[ a x a y a z ] = [ − g γ g θ g ]

[axayaz]" role="presentation" style="position: relative;">⎡⎣⎢axayaz⎤⎦⎥$$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$$

=

[−gγgθg]" role="presentation" style="position: relative;">⎡⎣⎢−gγgθg⎤⎦⎥$$\left[\begin{array}{c}-g\gamma \\ g\theta \\ g\end{array}\right]$$

​ax​ay​az​​ ​= ​−gγgθg​ ​
测量量只取$a_x$和$a_y$。
这样一来状态和测量方程都变得十分简单，计算量也大大减小，应用在51这样的单片机上也能在10ms内完成姿态解算。