MPU6050姿态解算与KF（5）可应用至TSC-51单片机

博客4中提到的滤波器状态维度为4维，而测量向量则为3维，每次滤波过程需要执行大量的矩阵乘法与求逆运算。若希望在51单片机上实现该算法，计算所需时间将显著增加。考虑到具体的应用环境，可以对滤波器进行适当简化，从而有效降低计算复杂度。
首先，陀螺仪与加速度计的数据融合仅能确保俯仰角和滚转角的收敛性，并且从测量方程的角度分析可知，加速度计的测量结果仅与俯仰角和滚转角相关。因此可以考虑将滤波器的状态变量选择为俯仰角\theta和滚转角\gamma，这样能够减少状态空间的维度。然而，选取这两个角度作为状态变量所带来的问题是状态方程必须依赖于欧拉角微分方程，而该方程中包含三角函数运算，反而会增加计算负担。
$\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\ \ome