卡尔曼滤波器 · 姿态解算 | 原理 / 公式推导 / 代码示例

注：本文为 “卡尔曼滤波器 · 姿态解算” 相关合辑。
略作重排，如有内容异常，请看原文。

---

浅谈基于卡尔曼滤波器的姿态解算（一）

争取35岁退休 已于 2024-04-24 21:44:28 修改

前言

在工程实践中，用于机器人姿态解算的算法种类较多，例如 Mahony 互补滤波算法、卡尔曼滤波算法等。此类算法均基于惯性测量单元（IMU）的输出数据实现，即利用 IMU 中的三轴加速度计与三轴角速度传感器（基于机体坐标系），通过坐标系转换关系完成姿态解算。（默认读者已掌握 IMU 的工作原理，相关内容不再赘述。）

卡尔曼滤波器

卡尔曼滤波器作为一种最优预测的数据融合算法，可有效融合加速度计与陀螺仪数据，获取理想的姿态信息。

对于线性状态系统：
$$\begin{array}{rl}\dot{\mathbf{X}}& =A\mathbf{X}+B\mathbf{U}\\ \mathbf{Y}& =C\mathbf{X}\end{array}$$

Step 1：利用上一时刻的后验误差协方差矩阵计算先验误差协方差矩阵
$$P_{k+1}^{-}=A{P}_k{A}^T+Q$$

Step 2：先验估计
$${\hat{x}}_{k+1}^{-}=A{\hat{x}}_k+B{u}_k$$

Step 3：计算卡尔曼增益
$$K_{k+1}=\frac{P_{k+1}^{-}{H}^T}{H{P}_{k+1}^{-}{H}^T+R}$$

Step 4：后验估计
$${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-H{\hat{x}}_{k+1}^{-})$$

Step 5：计算后验误差协方差矩阵
$$P_{k+1}=(I-K_{k+1}H)P_{k+1}^{-}$$

基于卡尔曼滤波器的姿态解算

卡尔曼滤波器使用方法

卡尔曼滤波器的应用需明确以下两点，再代入公式完成数据融合：

• 状态转移关系（先验估计）：确定状态转移方程的矩阵 $A$、$B$ 及系统噪声 $Q$；
• 状态测量值（由传感器观测获取）：确定观测值 ${\mathbf{z}}_{\mathbf{k}}$ 与测量噪声 $R$。

给定初始后验协方差矩阵 $P_0$，经迭代计算后，卡尔曼滤波器将收敛至最优区间，实现数据融合与预测。

IMU 数据解析

• 定义惯性系与机体系的坐标变换矩阵 $\mathbf{R}$（由惯性系至机体系），即：

$$\mathbf{R}={\mathbf{R}}_X(\varphi ){\mathbf{R}}_Y(\theta ){\mathbf{R}}_Z(\psi )=\left[\begin{array}{ccc}{C}_{\psi }{C}_{\theta }& S_{\psi }{C}_{\theta }& -S_{\theta }\\ C_{\psi }{S}_{\theta }{S}_{\varphi }-S_{\psi }{C}_{\varphi }& S_{\psi }{S}_{\theta }{S}_{\varphi }+C_{\psi }{C}_{\varphi }& C_{\theta }{S}_{\varphi }\\ C_{\psi }{S}_{\theta }{C}_{\varphi }+S_{\psi }{S}_{\varphi }& S_{\psi }{S}_{\theta }{C}_{\varphi }-C_{\psi }{S}_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right]$$

（注：式中 $C_{\cdot }=\cos (\cdot )$，$S_{\cdot }=\sin (\cdot )$。此处“点”符号（·）用作通配符或占位符，泛指任意角度变量。）

• 机体系下角速度向量 $[p,q,r{]}^T$ 与惯性系下欧拉角向量 $[\varphi ,\theta ,\psi {]}^T$ 满足特定关系（陀螺仪可提供欧拉角的变化率——欧拉角导数）。
  $$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& S_{\varphi }{T}_{\theta }& C_{\varphi }{T}_{\theta }\\ 0& C_{\varphi }& -S_{\varphi }\\ 0& S_{\varphi }/C_{\theta }& C_{\varphi }/C_{\theta }\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

• 静止或匀速运动状态下，加速度计采样的三轴数据为重力加速度在机体系中的投影。设惯性系下重力加速度向量为 $[0,0,-g{]}^T$，机体系下为 $[a_x,a_y,a_z{]}^T$，由坐标系转换的几何关系可得：
  $$\begin{array}{rl}\varphi & =\arctan (a_y/a_z)\\ \theta & =-\arctan (a_x/\sqrt{a_y^2+a_z^2})\end{array}$$
  上述几何关系基于 Z→Y→X 的旋转变换顺序推导，可通过绘制坐标系旋转变换示意图辅助理解。

卡尔曼滤波器模型

先假设初始的欧拉角为
$$\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$$
由于初始数据可能存在较大偏差，初始的后验协方差矩阵可设置为较大值，具体需根据实际情况调整。

step1：先验估计

先读取陀螺仪的角速度数据 $\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$，转换为单位 $\text{rad/s}$，代入以下公式得到欧拉角的导数 ${\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]}^T$：
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& S_{\varphi }{T}_{\theta }& C_{\varphi }{T}_{\theta }\\ 0& C_{\varphi }& -S_{\varphi }\\ 0& S_{\varphi }/C_{\theta }& C_{\varphi }/C_{\theta }\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\text{\hspace{1em}(1)}$$
（式中 $S_{\cdot }=\sin (\cdot )$，$C_{\cdot }=\cos (\cdot )$，$T_{\theta }=\tan (\theta )$）

将欧拉角的导数 ${\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]}^T$ 作为系统状态转移方程的输入向量 $\mathbf{U}$，即：
$$\mathbf{U}=\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\text{\hspace{1em}(2)}$$

设离散周期为 $\mathbf{T}$，离散系统的状态转移方程为：
$${\mathbf{X}}_{\mathbf{k}+1}=A{\mathbf{X}}_{\mathbf{k}}+B{\mathbf{U}}_{\mathbf{k}}$$

展开形式为：
$$\left[\begin{array}{c}{\hat{\varphi }}_{k+1}^{-}\\ {\hat{\theta }}_{k+1}^{-}\\ {\hat{\psi }}_{k+1}^{-}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{\varphi }}_k\\ {\hat{\theta }}_k\\ {\hat{\psi }}_k\end{array}\right]+\left[\begin{array}{ccc}T& 0& 0\\ 0& T& 0\\ 0& 0& T\end{array}\right]\left[\begin{array}{c}{\dot{\varphi }}_k\\ {\dot{\theta }}_k\\ {\dot{\psi }}_k\end{array}\right]\text{\hspace{1em}(3)}$$

其中，矩阵 $A$ 为三阶单位矩阵，矩阵 $B$ 为三阶单位矩阵乘以 $T$：

$$A=I_3=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],B=T\cdot I_3=\left[\begin{array}{ccc}T& 0& 0\\ 0& T& 0\\ 0& 0& T\end{array}\right]\text{\hspace{1em}(4)}$$

先验估计公式为：

$$\left[\begin{array}{c}{\hat{\varphi }}_{k+1}^{-}\\ {\hat{\theta }}_{k+1}^{-}\\ {\hat{\psi }}_{k+1}^{-}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{\varphi }}_k\\ {\hat{\theta }}_k\\ {\hat{\psi }}_k\end{array}\right]+\left[\begin{array}{ccc}T& 0& 0\\ 0& T& 0\\ 0& 0& T\end{array}\right]\left[\begin{array}{c}{\dot{\varphi }}_k\\ {\dot{\theta }}_k\\ {\dot{\psi }}_k\end{array}\right]\text{\hspace{1em}(5)}$$

先验误差协方差矩阵计算公式：
$$P_{k+1}^{-}=A{P}_k{A}^T+Q\text{\hspace{1em}(6)}$$

Step 2：状态测量值

状态测量值与系统状态满足关系：
$$z_k=H{\mathbf{X}}_{mk}$$

由加速度计数据转换得到的姿态角向量 $z_k$ 为：
$$z_k=\left[\begin{array}{c}{\varphi }_{mk}\\ {\theta }_{mk}\\ {\psi }_{mk}\end{array}\right]$$

通过三轴加速度计数据 $\mathbf{a}=[a_x,a_y,a_z{]}^T$ 计算：
$$\begin{array}{rl}{\varphi }_{mk}& =\arctan (a_y/a_z)\\ {\theta }_{mk}& =-\arctan (a_x/\sqrt{a_y^2+a_z^2})\end{array}\text{\hspace{1em}(7)}$$

（重力加速度数据无法解算航向角 $\psi$）

测量矩阵 $H$ 为：
$$H=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]$$

Step 3：后验估计

基于先验估计（公式 5）与先验协方差矩阵（公式 6），计算卡尔曼增益：
$$K_{k+1}=\frac{P_{k+1}^{-}{H}^T}{H{P}_{k+1}^{-}{H}^T+R}\text{\hspace{1em}(8)}$$

后验估计：
$${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-H{\hat{x}}_{k+1}^{-})\text{\hspace{1em}(9)}$$

后验误差协方差矩阵：
$$P_{k+1}=(I-K_{k+1}H)P_{k+1}^{-}\text{\hspace{1em}(10)}$$

总结

基于卡尔曼滤波器的 IMU 姿态解算流程可概括为：

1. 读取 IMU 的三轴角速度与三轴加速度数据；
2. 通过公式（1）计算欧拉角导数；
3. 通过公式（5）计算先验估计，公式（6）计算先验误差协方差矩阵；
4. 通过公式（7）获取状态测量值，公式（8）计算卡尔曼增益，代入公式（9）完成后验估计，公式（10）计算后验误差协方差矩阵，完成一个卡尔曼滤波周期。

---

浅谈基于卡尔曼滤波器的姿态解算（二）——代码实现及效果演示

争取35岁退休 已于 2024-04-25 02:53:38 修改

本文承接上篇内容，对基于卡尔曼滤波器的姿态解算进行代码实现与效果验证。主要内容包括算法公式解析、函数输入参数说明，并指出该解算方法可获取准确的横滚角与俯仰角，但航向角存在漂移问题，后续将引入磁力计予以解决。

前言

上篇博客完成了基于卡尔曼滤波器姿态解算的公式推导，本文将实现算法代码并演示解算效果。考虑到部分单片机不支持直接矩阵运算，需将矩阵运算拆解为方程组形式。

公式解析

Step 1：获取欧拉角导数

陀螺仪角速度数据 $\omega =[p,q,r{]}^T$ 与欧拉角导数的关系为：
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi /\cos \theta & \cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$$

离散形式方程组：
$$\{\begin{array}{rl}\dot{{\varphi }_k}& =p_k+\sin {\varphi }_k\tan {\theta }_k{q}_k+\cos {\varphi }_k\tan {\theta }_k{r}_k\\ \dot{{\theta }_k}& =\cos {\varphi }_k{q}_k-\sin {\varphi }_k{r}_k\\ \dot{{\psi }_k}& =\sin {\varphi }_k{q}_k/\cos {\theta }_k+\cos {\varphi }_k{r}_k/\cos {\theta }_k\end{array}\text{\hspace{1em}(1)}$$

Step 2：先验估计

先验估计公式：
$$\left[\begin{array}{c}{\hat{\varphi }}_{k+1}^{-}\\ {\hat{\theta }}_{k+1}^{-}\\ {\hat{\psi }}_{k+1}^{-}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\hat{\varphi }}_k\\ {\hat{\theta }}_k\\ {\hat{\psi }}_k\end{array}\right]+\left[\begin{array}{ccc}T& 0& 0\\ 0& T& 0\\ 0& 0& T\end{array}\right]\left[\begin{array}{c}{\dot{\varphi }}_k\\ {\dot{\theta }}_k\\ {\dot{\psi }}_k\end{array}\right]$$

离散形式方程组：
$$\{\begin{array}{rl}{\hat{\varphi }}_{k+1}^{-}& ={\hat{\varphi }}_k+T{\dot{\varphi }}_k\\ {\hat{\theta }}_{k+1}^{-}& ={\hat{\theta }}_k+T{\dot{\theta }}_k\\ {\hat{\psi }}_{k+1}^{-}& ={\hat{\psi }}_k+T{\dot{\psi }}_k\end{array}\text{\hspace{1em}(2)}$$

Step 3：先验估计误差协方差矩阵

设系统噪声协方差矩阵 $Q=\text{diag}(q_{\varphi },q_{\theta },q_{\psi })$，

先验误差协方差矩阵 $P_k^{-}=\text{diag}(p_{\varphi ,k}^{-},p_{\theta ,k}^{-},p_{\psi ,k}^{-})$，

后验误差协方差矩阵 $P_k=\text{diag}(p_{\varphi ,k},p_{\theta ,k},p_{\psi ,k})$，则：

$$\{\begin{array}{rl}{p}_{\varphi ,k+1}^{-}& =p_{\varphi ,k}+q_{\varphi }\\ p_{\theta ,k+1}^{-}& =p_{\theta ,k}+q_{\theta }\\ p_{\psi ,k+1}^{-}& =p_{\psi ,k}+q_{\psi }\end{array}\text{\hspace{1em}(3)}$$

Step 4：卡尔曼增益矩阵计算

测量矩阵 $H=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]$，测量噪声协方差矩阵 $R=\text{diag}(r_{\varphi },r_{\theta },r_{\psi })$，卡尔曼增益矩阵 $K_k=\text{diag}(k_{1,k},k_{2,k},k_{3,k})$，则：
$$\{\begin{array}{rl}{k}_{1,k+1}& =p_{\varphi ,k+1}^{-}/(p_{\varphi ,k+1}^{-}+r_{\varphi })\\ k_{2,k+1}& =p_{\theta ,k+1}^{-}/(p_{\theta ,k+1}^{-}+r_{\theta })\\ k_{3,k+1}& =0\end{array}\text{\hspace{1em}(4)}$$

Step 5：获取欧拉角测量值

测量向量 $z_k=\left[\begin{array}{c}{\varphi }_{m,k}\\ {\theta }_{m,k}\\ {\psi }_{m,k}\end{array}\right]$，其中：
$$\{\begin{array}{rl}{\varphi }_{m,k}& =\arctan (a_y/a_z)\\ {\theta }_{m,k}& =-\arctan (a_x/\sqrt{a_y^2+a_z^2})\\ {\psi }_{m,k}& =0\end{array}\text{\hspace{1em}(5)}$$
其中，$[a_x,a_y,a_z{]}^T$ 是三轴加速度计数据。

step 6：后验估计计算

后验估计的计算公式为：
$${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-H{\hat{x}}_{k+1}^{-})$$

令后验估计状态为 $\left[\begin{array}{c}{\hat{\varphi }}_{k+1}\\ {\hat{\theta }}_{k+1}\\ {\hat{\psi }}_{k+1}\end{array}\right]$，可得离散形式方程组：
$$\{\begin{array}{rl}{\hat{\varphi }}_{k+1}& =(1-k_{1,k+1}){\hat{\varphi }}_{k+1}^{-}+k_{1,k+1}{\varphi }_{m,k+1}\\ {\hat{\theta }}_{k+1}& =(1-k_{2,k+1}){\hat{\theta }}_{k+1}^{-}+k_{2,k+1}{\theta }_{m,k+1}\\ {\hat{\psi }}_{k+1}& ={\hat{\psi }}_{k+1}^{-}\end{array}\text{\hspace{1em}(6)}$$

航向角漂移问题源于未引入测量数据校正，仅沿用先验估计结果，导致陀螺仪累计误差使航向角逐渐漂移。其根本原因是测量值中 ${\psi }_{m,k}=0$，无法对航向角进行校正。

Step 7：后验估计误差协方差矩阵

$$\{\begin{array}{rl}{p}_{\varphi ,k+1}& =(1-k_{1,k+1})p_{\varphi ,k+1}^{-}\\ p_{\theta ,k+1}& =(1-k_{2,k+1})p_{\theta ,k+1}^{-}\\ p_{\psi ,k+1}& =p_{\psi ,k+1}^{-}\end{array}\text{\hspace{1em}(7)}$$

代码实现

函数接口说明

输入参数：

• $gx,gy,gz$：陀螺仪角速度数据，单位 $\text{rad/s}$；
• $ax,ay,az$：加速度计数据（单位不限）；

（需确保角速度坐标系与加速度坐标系方向一致）

#include <math.h>
float roll = 0, pitch = 0, yaw = 0; 
// 欧拉角存储变量
static float Xk_[3] = {0};  
// 先验估计值
static float Xk[3] = {0};
// 后验估计值
static float Uk[3] = {0};
// 系统输入向量
static float Zk[3] = {0};
// 测量状态向量
static float Pk[3] = {1, 1, 1};
// 后验误差协方差矩阵
static float Pk_[3] = {0};
// 先验误差协方差矩阵
static float K[3] = {0};
// 卡尔曼增益矩阵
static float Q[3] = {1, 1, 1};
// 系统噪声协方差矩阵
static float R[3] = {1, 1, 1};
// 测量噪声协方差矩阵
static float T = 0.002f;
// 离散周期

// 基于卡尔曼滤波器的姿态解算函数
void kalman_filter_attitude_solution(float gx, float gy, float gz, float ax, float ay, float az)
{
    // Step 1：计算系统输入向量（欧拉角导数）
    Uk[0] = gx + sin(Xk[0]) * tan(Xk[1]) * gy + cos(Xk[0]) * tan(Xk[1]) * gz;
    Uk[1] = cos(Xk[0]) * gy - sin(Xk[0]) * gz;
    Uk[2] = sin(Xk[0]) * gy / cos(Xk[1]) + cos(Xk[0]) * gz / cos(Xk[1]);

    // Step 2：先验估计
    Xk_[0] = Xk[0] + T * Uk[0];
    Xk_[1] = Xk[1] + T * Uk[1];
    Xk_[2] = Xk[2] + T * Uk[2];

    // Step 3：先验误差协方差矩阵
    Pk_[0] = Pk[0] + Q[0];
    Pk_[1] = Pk[1] + Q[1];
    Pk_[2] = Pk[2] + Q[2];

    // Step 4：计算卡尔曼增益
    K[0] = Pk_[0] / (Pk_[0] + R[0]);
    K[1] = Pk_[1] / (Pk_[1] + R[1]);
    K[2] = 0;

    // Step 5：获取测量数据
    Zk[0] = atan(ay / az);
    Zk[1] = -atan(ax / (sqrt(ay * ay + az * az)));
    Zk[2] = 0;

    // Step 6：后验估计
    Xk[0] = (1 - K[0]) * Xk_[0] + K[0] * Zk[0];
    Xk[1] = (1 - K[1]) * Xk_[1] + K[1] * Zk[1];
    Xk[2] = Xk_[2];

    // Step 7：后验误差协方差矩阵
    Pk[0] = (1 - K[0]) * Pk_[0];
    Pk[1] = (1 - K[1]) * Pk_[1];
    Pk[2] = Pk_[2];

    // 转换为角度制
    roll = Xk[0] / 3.141f * 180.f;
    pitch = Xk[1] / 3.141f * 180.f;
    yaw = Xk[2] / 3.141f * 180.f;
}

效果演示

采用匿名飞控板与匿名上位机验证姿态解算效果（非商业推广，仅为实验器材）。

总结

• 基于卡尔曼滤波器的姿态解算可获得高精度的横滚角与俯仰角，但航向角因缺乏测量校正存在漂移，且在振动、电磁干扰环境下漂移加剧；
• 初始化时可通过计算平均偏移量进行偏置补偿，但无法彻底消除漂移；
• 后续将引入磁力计传感器解决航向角漂移问题。

---

浅谈基于卡尔曼滤波器的姿态解算（三）——磁力计标定航向角

争取35岁退休 已于 2024-04-25 16:41:50 修改

前言

经过前两篇博客的研究，第一篇基于卡尔曼滤波器完成了姿态解算的原理推导，第二篇实现了算法代码并验证了解算效果。

但第二篇博客发现航向角存在漂移问题，分析原因是重力加速度数据无法对航向角 yaw 进行标定，导致陀螺仪数据噪声在姿态解算过程中累积误差，引发航向角 yaw 漂移。

针对这一问题，本文将引入磁力计传感器来标定航向角 yaw，并介绍相关原理和公式推导过程。

磁力计传感器

原理

磁力计（电子罗盘）基于地球磁场确定惯性系方向，其原理与指南针类似。地球磁感线由地磁南极指向地磁北极，磁力计将磁感线分解为机体系 $XYZ$ 三轴分量，利用该数据标定航向角，解决漂移问题。

问题

磁力计由三组霍尔元件构成，受设计、加工等因素影响，输出数据常存在不对称性：

• 理想情况下，机体系转动时磁力计三轴数据应分布于以坐标原点为中心的球面上；
• 实际输出数据分布于中心偏离原点的椭球面上，需对数据进行归一化校正。

磁力计标定航向角原理

惯性系定义

惯性系 $XYZ$ 轴分别对应北、东、天（NED 坐标系），地球磁感线位于惯性系 $XOZ$ 平面内（由南向北），其在 $X$ 轴与 $Z$ 轴的分量比例随地理位置变化：赤道附近 $X$ 轴分量占优，两极附近 $Z$ 轴分量占优。

通常定义正北方向为航向角零度（$\psi =0$），航向角正负由右手法则判定。

公式推导

在不考虑磁场干扰的前提下，地球磁场在惯性系中的向量可表示为 $\left[\begin{array}{c}{m}_{e,x}\\ 0\\ m_{e,z}\end{array}\right]$；经坐标系变换后，其在机体系中的向量为 $\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]$。其中，从惯性系到机体系的坐标系变换矩阵 $\mathbf{R}$ 定义如下：

$$\mathbf{R}={\mathbf{R}}_X(\varphi ){\mathbf{R}}_Y(\theta ){\mathbf{R}}_Z(\psi )=\left[\begin{array}{ccc}{C}_{\psi }{C}_{\theta }& S_{\psi }{C}_{\theta }& -S_{\theta }\\ C_{\psi }{S}_{\theta }{S}_{\varphi }-S_{\psi }{C}_{\varphi }& S_{\psi }{S}_{\theta }{S}_{\varphi }+C_{\psi }{C}_{\varphi }& C_{\theta }{S}_{\varphi }\\ C_{\psi }{S}_{\theta }{C}_{\varphi }+S_{\psi }{S}_{\varphi }& S_{\psi }{S}_{\theta }{C}_{\varphi }-C_{\psi }{S}_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right]\text{\hspace{1em}(1)}$$

式中各旋转矩阵的定义为：
$${\mathbf{R}}_Z(\psi )=\left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$

$${\mathbf{R}}_Y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right]\text{\hspace{1em}(3)}$$

$${\mathbf{R}}_X(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & \sin \varphi \\ 0& -\sin \varphi & \cos \varphi \end{array}\right]\text{\hspace{1em}(4)}$$

（说明：上述表达式中，$C_{\cdot }$ 代表 $\cos (\cdot )$，$S_{\cdot }$ 代表 $\sin (\cdot )$）

机体系磁场向量与惯性系磁场向量满足：
$$\begin{array}{rl}\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]& =\mathbf{R}\left[\begin{array}{c}{m}_{e,x}\\ 0\\ m_{e,z}\end{array}\right]={\mathbf{R}}_X(\varphi ){\mathbf{R}}_Y(\theta ){\mathbf{R}}_Z(\psi )\left[\begin{array}{c}{m}_{e,x}\\ 0\\ m_{e,z}\end{array}\right]\end{array}\text{\hspace{1em}(5)}$$

为解算航向角 $\psi$，定义惯性系磁场向量绕 $Z$ 轴旋转 $\psi$ 后的向量为 $\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]$：

$$\begin{array}{rl}\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]& ={\mathbf{R}}_Z(\psi )\left[\begin{array}{c}{m}_{e,x}\\ 0\\ m_{e,z}\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$

在分析地球磁场的坐标系变换时，为了降低计算复杂度，会选择惯性系的坐标轴方向：将惯性系的 $y$ 轴调整至“与地球磁场向量垂直”的方向，此时地球磁场在该系的 $y$ 方向上没有投影，因此其 $y$ 分量自然为 0。

由几何关系得航向角：

$$\begin{array}{rl}\psi & =\text{atan}\left(\frac{m_{Z,x}}{m_{Z,y}}\right)\end{array}\text{\hspace{1em}(7)}$$
联立公式（5）与（6），可得：
$$\begin{array}{rl}\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]& ={\mathbf{R}}_X(\varphi ){\mathbf{R}}_Y(\theta )\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]\end{array}\text{\hspace{1em}(8)}$$
整理后得到：

$$\begin{array}{rl}\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]& ={\mathbf{R}}_Y^{-1}(\theta ){\mathbf{R}}_X^{-1}(\varphi )\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]\end{array}\text{\hspace{1em}(9)}$$

由公式 3 和公式 4 所得旋转变换矩阵为正交矩阵，其逆等于转置：

$$\begin{array}{rl}{\mathbf{R}}_Y^{-1}(\theta )& ={\mathbf{R}}_Y^T(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\\ & \\ {\mathbf{R}}_X^{-1}(\varphi )& ={\mathbf{R}}_X^T(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right]\end{array}\text{\hspace{1em}(10)(11)}$$

将 公式10 和 公式11 代入公式（9）得：

$$\begin{array}{rl}\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]& =\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi & \cos \varphi \end{array}\right]\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]\end{array}\text{\hspace{1em}(12)}$$

整理为（记 $C_{\theta }=\cos \theta ,S_{\theta }=\sin \theta ;C_{\varphi }=\cos \varphi ,S_{\varphi }=\sin \varphi$）：
$$\begin{array}{rl}\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]& =\left[\begin{array}{ccc}{C}_{\theta }& S_{\theta }{S}_{\varphi }& S_{\theta }{C}_{\varphi }\\ 0& C_{\varphi }& -S_{\varphi }\\ -S_{\theta }& C_{\theta }{S}_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right]\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]\end{array}\text{\hspace{1em}(13)}$$

代码实现

step1：读取并处理磁力计数据

令读取到的磁力计原始数据为 $\left[\begin{array}{c}{m}_{raw,x}\\ m_{raw,y}\\ m_{raw,z}\end{array}\right]$，分别绕 $X,Y,Z$ 轴完整旋转一圈，记录各轴上的最大及最小值，记为 $\left[\begin{array}{c}{m}_{max,x}\\ m_{max,y}\\ m_{max,z}\end{array}\right]$ 和 $\left[\begin{array}{c}{m}_{min,x}\\ m_{min,y}\\ m_{min,z}\end{array}\right]$。

设校准后的磁力计数据为 $\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]$，满足以下关系：
$$\{\begin{array}{l}{m}_{b,x}=\frac{m_{raw,x}-(m_{max,x}+m_{min,x})/2}{(m_{max,x}-m_{min,x})/2}\\ m_{b,y}=\frac{m_{raw,y}-(m_{max,y}+m_{min,y})/2}{(m_{max,y}-m_{min,y})/2}\\ m_{b,z}=\frac{m_{raw,z}-(m_{max,z}+m_{min,z})/2}{(m_{max,z}-m_{min,z})/2}\end{array}$$

step2：计算由磁力计解算的航向角

由公式 13 可得：
$$\{\begin{array}{l}{m}_{Z,x}=C_{\theta }{m}_{b,x}+S_{\theta }{S}_{\varphi }{m}_{b,y}+S_{\theta }{C}_{\varphi }{m}_{b,z}\\ m_{Z,y}=C_{\varphi }{m}_{b,y}-S_{\varphi }{m}_{b,z}\\ m_{Z,z}=-S_{\theta }{m}_{b,x}+C_{\theta }{S}_{\varphi }{m}_{b,y}+C_{\theta }{C}_{\varphi }{m}_{b,z}\end{array}\text{\hspace{1em}(14)}$$

由公式 7 得到航向角在卡尔曼滤波器中的测量值，即：
$${\psi }_{m,k}=\text{atan}(m_{Z,x}/m_{Z,y})\text{\hspace{1em}(15)}$$

在上一篇的基础上修改测量状态向量 ${\hat{z}}_k$ 以及测量矩阵 $H$，即令：
$$\{\begin{array}{l}{\varphi }_{m,k}=\text{atan}(a_y/a_x)\\ {\theta }_{m,k}=-\text{atan}\left(a_z/\sqrt{a_y^2+a_x^2}\right)\\ {\psi }_{m,k}=\text{atan}(m_{Z,y}/m_{Z,x})\end{array}\text{\hspace{1em}(16)}$$
$$H=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(17)}$$

step3：完成卡尔曼滤波计算

一、状态转移

陀螺仪数据计算欧拉角变化率

$$\{\begin{array}{l}{\dot{\varphi }}_k=p_k+S_{{\varphi }_k}{T}_{{\theta }_k}{q}_k+C_{{\varphi }_k}{T}_{{\theta }_k}{r}_k\\ {\dot{\theta }}_k=C_{{\varphi }_k}{q}_k-S_{{\varphi }_k}{r}_k\\ {\dot{\psi }}_k=S_{{\varphi }_k}{q}_k/C_{{\theta }_k}+C_{{\varphi }_k}{r}_k/C_{{\theta }_k}\end{array}$$

二、先验估计计算

$$\{\begin{array}{l}{\hat{\varphi }}_{k+1}^{-}={\hat{\varphi }}_k+T{\dot{\varphi }}_k\\ {\hat{\theta }}_{k+1}^{-}={\hat{\theta }}_k+T{\dot{\theta }}_k\\ {\hat{\psi }}_{k+1}^{-}={\hat{\psi }}_k+T{\dot{\psi }}_k\end{array}$$

三、先验估计误差协方差矩阵计算

$$\{\begin{array}{l}{P}_{\varphi ,k+1}^{-}=P_{\varphi ,k}+q_{\varphi }\\ P_{\theta ,k+1}^{-}=P_{\theta ,k}+q_{\theta }\\ P_{\psi ,k+1}^{-}=P_{\psi ,k}+q_{\psi }\end{array}$$

四、卡尔曼增益矩阵计算

$$\{\begin{array}{l}{k}_{1,k+1}=P_{\varphi ,k+1}^{-}/(P_{\varphi ,k+1}^{-}+r_{\varphi })\\ k_{2,k+1}=P_{\theta ,k+1}^{-}/(P_{\theta ,k+1}^{-}+r_{\theta })\\ k_{3,k+1}=P_{\psi ,k+1}^{-}/(P_{\psi ,k+1}^{-}+r_{\psi })\end{array}$$

五、后验估计计算（测量状态向量参考公式 16）

$$\{\begin{array}{l}{\hat{\varphi }}_{k+1}=(1-k_{1,k+1}){\hat{\varphi }}_{k+1}^{-}+k_{1,k+1}{\varphi }_{m,k+1}\\ {\hat{\theta }}_{k+1}=(1-k_{2,k+1}){\hat{\theta }}_{k+1}^{-}+k_{2,k+1}{\theta }_{m,k+1}\\ {\hat{\psi }}_{k+1}=(1-k_{3,k+1}){\hat{\psi }}_{k+1}^{-}+k_{3,k+1}{\psi }_{m,k+1}\end{array}$$

六、后验估计误差协方差矩阵

$$\{\begin{array}{l}{P}_{\varphi ,k+1}=(1-k_{1,k+1})P_{\varphi ,k+1}^{-}\\ P_{\theta ,k+1}=(1-k_{2,k+1})P_{\theta ,k+1}^{-}\\ P_{\psi ,k+1}=(1-k_{3,k+1})P_{\psi ,k+1}^{-}\end{array}$$

上述步骤的 C 语言实现

函数接口说明

函数输入参数：

• gx, gy, gz：陀螺仪角速度数据，单位：$\text{rad/s}$；
• ax, ay, az：加速度计数据，单位不管；
• mx, my, mz：磁力计数据，单位不管；

注意：要将三组传感器的坐标系对应上，统一方向。

要不要我帮你补充一份传感器坐标系统一的校验要点，避免数据方向错误？

C 语言实现代码

#include <math.h>
#include <float.h>

float roll = 0, pitch = 0, yaw = 0; 
// Euler angle storage variables
static float Xk_[3] = {0};  
// A priori estimate values
static float Xk[3] = {0};
// A posteriori estimate values
static float Uk[3] = {0};
// System input vector
static float Zk[3] = {0};
// Measurement state vector
static float Pk[3] = {1, 1, 1};
// A posteriori error covariance matrix
static float Pk_[3] = {0};
// A priori error covariance matrix
static float K[3] = {0};
// Kalman gain matrix
static float Q[3] = {1, 1, 1};
// System noise covariance matrix
static float R[3] = {1, 1, 1};
// Measurement noise covariance matrix
static float T = 0.002f;
// Discrete period

// Magnetometer calibration parameters (example values, need actual calibration)
float mx_max = 30.0f, mx_min = -24.9f;
float my_max = 25.2f, my_min = -30.15f;
float mz_max = 33.3f, mz_min = -23.25f;

// Kalman filter attitude solution function with magnetometer calibration for yaw
void kalman_filter_attitude_solution_calibyaw(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
    // Step 1: Calculate system input vector (Euler angle derivatives)
    Uk[0] = gx + sin(Xk[0]) * tan(Xk[1]) * gy + cos(Xk[0]) * tan(Xk[1]) * gz;
    Uk[1] = cos(Xk[0]) * gy - sin(Xk[0]) * gz;
    Uk[2] = sin(Xk[0]) * gy / cos(Xk[1]) + cos(Xk[0]) * gz / cos(Xk[1]);

    // Step 2: A priori estimation
    Xk_[0] = Xk[0] + T * Uk[0];
    Xk_[1] = Xk[1] + T * Uk[1];
    Xk_[2] = Xk[2] + T * Uk[2];

    // Yaw angle range constraint (-π~π)
    if (Xk_[2] > 3.141f) Xk_[2] -= 2 * 3.141f;
    else if (Xk_[2] < -3.141f) Xk_[2] += 2 * 3.141f;

    // Step 3: Calculate a priori error covariance matrix
    Pk_[0] = Pk[0] + Q[0];
    Pk_[1] = Pk[1] + Q[1];
    Pk_[2] = Pk[2] + Q[2];

    // Step 4: Calculate Kalman gain
    K[0] = Pk_[0] / (Pk_[0] + R[0]);
    K[1] = Pk_[1] / (Pk_[1] + R[1]);
    K[2] = Pk_[2] / (Pk_[2] + R[2]);

    // Step 5: Magnetometer data calibration and yaw angle calculation
    float mbx, mby, mbz;
    float mZx, mZy, mZz;

    mbx = (mx - (mx_max + mx_min) / 2.f) / ((mx_max - mx_min) / 2.f);
    mby = (my - (my_max + my_min) / 2.f) / ((my_max - my_min) / 2.f);
    mbz = (mz - (mz_max + mz_min) / 2.f) / ((mz_max - mz_min) / 2.f);

    mZx = cos(Xk[1]) * mbx + sin(Xk[1]) * sin(Xk[0]) * mby + sin(Xk[1]) * cos(Xk[0]) * mbz;
    mZy = cos(Xk[0]) * mby - sin(Xk[0]) * mbz;
    mZz = -sin(Xk[1]) * mbx + cos(Xk[1]) * sin(Xk[0]) * mby + cos(Xk[1]) * cos(Xk[0]) * mbz;

    // Step 6: Get measurement data
    Zk[0] = atan(ay / az);
    Zk[1] = -atan(ax / (sqrt(ay * ay + az * az)));
    Zk[2] = atan2(mZy, mZx);

    // Step 7: A posteriori estimation
    Xk[0] = (1 - K[0]) * Xk_[0] + K[0] * Zk[0];
    Xk[1] = (1 - K[1]) * Xk_[1] + K[1] * Zk[1];
    Xk[2] = (1 - K[2]) * Xk_[2] + K[2] * Zk[2];

    // Step 8: Update a posteriori error covariance matrix
    Pk[0] = (1 - K[0]) * Pk_[0];
    Pk[1] = (1 - K[1]) * Pk_[1];
    Pk[2] = (1 - K[2]) * Pk_[2];

    // Convert to degrees
    roll = Xk[0] / 3.141f * 180.f;
    pitch = Xk[1] / 3.141f * 180.f;
    yaw = Xk[2] / 3.141f * 180.f;
}

总结

引入磁力计的卡尔曼滤波姿态解算与无磁力计方案的区别：

• 无磁力计方案以上电时的航向为零度，输出相对航向角；
• 带磁力计方案基于地球磁场输出绝对航向角。

注

博客中基于卡尔曼滤波器的姿态解算实现代码，主要侧重于算法推导过程的具象化呈现，尚未针对工程应用场景进行深度优化，使用者可结合具体的硬件平台与实际需求开展针对性的性能优化工作。

相较于 Mahony 互补滤波等其他惯性导航姿态解算算法，卡尔曼滤波器通过多步骤的矩阵运算与误差协方差迭代完成数据融合，其算法逻辑复杂度更高，在实际运行过程中对硬件算力资源的占用量相对较大，导致其在部分资源受限场景下的运行效率略低。

---

via：

• 浅谈基于卡尔曼滤波器的姿态解算（一）_卡尔曼滤波姿态解算-优快云博客
  https://blog.youkuaiyun.com/m0_37835056/article/details/137972765

  • 卡尔曼滤波解析-优快云博客
    https://blog.youkuaiyun.com/m0_37835056/article/details/127284758

• 浅谈基于卡尔曼滤波器的姿态解算（二）——代码实现及效果演示_卡尔曼姿态解算代码-优快云博客
  https://blog.youkuaiyun.com/m0_37835056/article/details/138134235

• 浅谈基于卡尔曼滤波器的姿态解算（三）——磁力计标定航向角_航向角卡尔曼滤波-优快云博客
  https://blog.youkuaiyun.com/m0_37835056/article/details/138168711