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

---

---

前言

• 第一篇博客对基于卡尔曼滤波器的姿态解算进行了原理简述和公式推导，传送门
• 第二篇博客在第一篇的基础上实现代码并观察解算效果，传送门
• 在第二篇博客中，提到了由于重力加速度数据无法对航向角yaw进行标定，导致陀螺仪数据噪声会在姿态解算过程中累积误差，使航向角yaw漂移。
• 本博客将引入磁力计传感器，来对航向角yaw进行标定，并介绍相关原理和公式推导过程。

磁力计传感器

原理

磁力计又称电子罗盘，其与指南针原理相似，都是通过地球磁场信息来确定惯性系方向。

磁感线从地磁南极指向地磁北极，而磁力计会将经过其的磁感线分解为机体系$XYZ$三个轴的分量。利用这组数据，与加速度计类似的原理，用来对航向角进行标定，从而获得准确且稳定的航向信息，最终解决航向角漂移问题。

问题

磁力计本身是由三组霍尔元件堆叠而成，因此由于设计或加工等问题，常常会出现磁力计输出数据不对称的问题。

• 地球磁场向量在惯性系 中是一个固定的向量，因此当机体系转动时，在理想情况下，磁力计三轴数据应始终位于一个中心点在坐标原点的球面上。
• 但实际上磁力计三轴数据输出产生的面是一个中心点不在坐标原点的椭球面，并且根据不同的芯片情况和工况环境，产生的椭球面不同。
  因此在进行数据读取后需要对三轴磁力计数据进行归一化处理，后续详述。

磁力计标定航向角原理

惯性系定义

通常，惯性系的$XYZ$轴分别对应北东天，即惯性系$X$轴指向北，$Y$轴指向东，$Z$轴指向天。因此，地球磁感线方向向量始终在惯性系平面$XOZ$中磁感线始终由南指向北，其在$X$轴和$Z$轴的分量比例取决于在地球的位置：

• 若在地球赤道附近，则$X$轴占比较大；
• 若在南北极附近，则$Z$轴占比较大；

综上，通常定义正北方向为零度航向角，即$yaw=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]$，其中从惯性系到机体系的坐标系变换矩阵$R$定义为：
$$\mathbf{R}={\mathbf{R}}_{\mathbf{X}}(\mathbf{\varphi }){\mathbf{R}}_{\mathbf{Y}}(\mathbf{\theta }){\mathbf{R}}_{\mathbf{Z}}(\mathbf{\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}}_{\mathbf{Z}}(\mathbf{\psi })=\left[\begin{array}{ccc}cos\mathbf{\psi }& sin\mathbf{\psi }& 0\\ -sin\mathbf{\psi }& cos\mathbf{\psi }& 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
$${\mathbf{R}}_{\mathbf{Y}}(\mathbf{\theta })=\left[\begin{array}{ccc}cos\mathbf{\theta }& 0& -sin\mathbf{\theta }\\ 0& 1& 0\\ sin\mathbf{\theta }& 0& cos\mathbf{\theta }\end{array}\right]\text{\hspace{1em}(3)}$$
$${\mathbf{R}}_{\mathbf{X}}(\mathbf{\varphi })=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathbf{\varphi }& sin\mathbf{\varphi }\\ 0& -sin\mathbf{\varphi }& cos\mathbf{\varphi }\end{array}\right]\text{\hspace{1em}(4)}$$
好了，有了上面这些前提，我要开始表演（公式推导）了
根据上述描述，满足以下关系：
$$\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}}_{\mathbf{X}}(\mathbf{\varphi }){\mathbf{R}}_{\mathbf{Y}}(\mathbf{\theta }){\mathbf{R}}_{\mathbf{Z}}(\mathbf{\psi })\left[\begin{array}{c}{m}_{e,x}\\ 0\\ m_{e,z}\end{array}\right]\text{\hspace{1em}(5)}$$
由于坐标系是按顺序绕轴转动的，而此时磁力计数据的目标是为了解算得到航向角$\mathbf{\psi }$，因此我们如果能得到惯性系下磁力数据绕$Z$轴转动$\mathbf{\psi }$角后的向量$\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]$，满足以下关系
$$\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]={\mathbf{R}}_{\mathbf{Z}}(\mathbf{\psi })\left[\begin{array}{c}{m}_{e,x}\\ 0\\ m_{e,z}\end{array}\right]\text{\hspace{1em}(6)}$$
根据坐标系变换的几何关系，可以得到以下关系，
$$\mathbf{\psi }=atan(m_{Z,x}/m_{Z,y})\text{\hspace{1em}(7)}$$
由 公式5 和 公式6 可以得到：
$$\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]={\mathbf{R}}_{\mathbf{X}}(\mathbf{\varphi }){\mathbf{R}}_{\mathbf{Y}}(\mathbf{\theta })\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]\text{\hspace{1em}(8)}$$
整理后得到：
$$\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]={\mathbf{R}}_{\mathbf{Y}}^{-1}(\mathbf{\theta }){\mathbf{R}}_{\mathbf{X}}^{-1}(\mathbf{\varphi })\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]\text{\hspace{1em}(9)}$$
由 公式3 和 公式4 所得这几个旋转变换矩阵都是正交矩阵
$${\mathbf{R}}_{\mathbf{Y}}^{-1}(\mathbf{\theta })={\mathbf{R}}_{\mathbf{Y}}^{\mathbf{T}}(\mathbf{\theta })=\left[\begin{array}{ccc}cos\mathbf{\theta }& 0& sin\mathbf{\theta }\\ 0& 1& 0\\ -sin\mathbf{\theta }& 0& cos\mathbf{\theta }\end{array}\right]\text{\hspace{1em}(10)}$$
$${\mathbf{R}}_{\mathbf{X}}^{-1}(\mathbf{\varphi })={\mathbf{R}}_{\mathbf{X}}^{\mathbf{T}}(\mathbf{\varphi })=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathbf{\varphi }& -sin\mathbf{\varphi }\\ 0& sin\mathbf{\varphi }& cos\mathbf{\varphi }\end{array}\right]\text{\hspace{1em}(11)}$$
将 公式10 和 公式11 代入 公式9，得到：
$$\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]=\left[\begin{array}{ccc}cos\mathbf{\theta }& 0& sin\mathbf{\theta }\\ 0& 1& 0\\ -sin\mathbf{\theta }& 0& cos\mathbf{\theta }\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathbf{\varphi }& -sin\mathbf{\varphi }\\ 0& sin\mathbf{\varphi }& cos\mathbf{\varphi }\end{array}\right]\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]\text{\hspace{1em}(12)}$$
整理为：
$$\left[\begin{array}{c}{m}_{Z,x}\\ m_{Z,y}\\ m_{Z,z}\end{array}\right]=\left[\begin{array}{ccc}{C}_{\mathbf{\theta }}& S_{\mathbf{\theta }}{S}_{\mathbf{\varphi }}& S_{\mathbf{\theta }}{C}_{\mathbf{\varphi }}\\ 0& C_{\mathbf{\varphi }}& -S_{\mathbf{\varphi }}\\ -S_{\mathbf{\theta }}& C_{\mathbf{\theta }}{S}_{\mathbf{\varphi }}& C_{\mathbf{\theta }}{C}_{\mathbf{\varphi }}\end{array}\right]\left[\begin{array}{c}{m}_{b,x}\\ m_{b,y}\\ m_{b,z}\end{array}\right]\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}{rl}& m_{b,x}=(m_{raw,x}-(m_{max,x}+m_{min,x})/2)/(m_{max,x}-m_{min,x})/2)\\ & m_{b,y}=(m_{raw,y}-(m_{max,y}+m_{min,y})/2)/(m_{max,y}-m_{min,y})/2)\\ & m_{b,z}=(m_{raw,z}-(m_{max,z}+m_{min,z})/2)/(m_{max,z}-m_{min,z})/2)\end{array}$$
• $step2$：计算由磁力计解算到的航向角
  由 公式13 可得
  $$\{\begin{array}{rl}& m_{Z,x}=C_{\mathbf{\theta }}{m}_{b,x}+S_{\mathbf{\theta }}{S}_{\mathbf{\varphi }}{m}_{b,y}+S_{\mathbf{\theta }}{C}_{\mathbf{\varphi }}{m}_{b,z}\\ & m_{Z,y}=C_{\mathbf{\varphi }}{m}_{b,y}-S_{\mathbf{\varphi }}{m}_{b,z}\\ & m_{Z,z}=-S_{\mathbf{\theta }}{m}_{b,x}+C_{\mathbf{\theta }}{S}_{\mathbf{\varphi }}{m}_{b,y}+C_{\mathbf{\theta }}{C}_{\mathbf{\varphi }}{m}_{b,z}\end{array}\text{\hspace{1em}(14)}$$
  由 公式7 得到航向角在卡尔曼滤波器中的测量值，即：
  $${\psi }_{m,k}=atan(m_{Z,x}/m_{Z,y})\text{\hspace{1em}(15)}$$
  在上一篇的基础上修改测量状态向量$z_k$以及测量矩阵$H$，即令
  $$\{\begin{array}{rl}& {\varphi }_{m,k}=atan(a_y/a_z)\\ & {\theta }_{m,k}=-atan(a_x/\sqrt{a_y^2+a_z^2})\\ & {\psi }_{m,k}=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}{rl}& \dot{{\mathbf{\varphi }}_{\mathbf{k}}}=p_k+S_{{\varphi }_k}{T}_{{\theta }_k}{q}_k+C_{{\varphi }_k}{T}_{{\theta }_k}{r}_k\\ & \dot{{\mathbf{\theta }}_{\mathbf{k}}}=C_{{\varphi }_k}{q}_k-S_{{\varphi }_k}{r}_k\\ & \dot{{\mathbf{\psi }}_{\mathbf{k}}}=S_{{\varphi }_k}{q}_k/C_{{\theta }_k}+C_{{\varphi }_k}{r}_k/C_{{\theta }_k}\end{array}$$
  二、先验估计计算
  $$\{\begin{array}{rl}& {\hat{\mathbf{\varphi }}}_{\mathbf{k}+1}^{-}={\hat{\mathbf{\varphi }}}_{\mathbf{k}}+T\dot{{\mathbf{\varphi }}_{\mathbf{k}}}\\ & {\hat{\mathbf{\theta }}}_{\mathbf{k}+1}^{-}={\hat{\mathbf{\theta }}}_{\mathbf{k}}+T\dot{{\mathbf{\theta }}_{\mathbf{k}}}\\ & {\hat{\mathbf{\psi }}}_{\mathbf{k}+1}^{-}={\hat{\mathbf{\psi }}}_{\mathbf{k}}+T\dot{{\mathbf{\psi }}_{\mathbf{k}}}\end{array}$$
  三、先验估计误差协方差矩阵计算
  $$\{\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}$$
  四、卡尔曼增益矩阵计算
  $$\{\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}=p_{\psi ,k+1}^{-}/(p_{\psi ,k+1}^{-}+r_{\psi })\end{array}$$
  五、后验估计计算（测量状态向量参考公式16）
  $$\{\begin{array}{rl}& {\hat{\mathbf{\varphi }}}_{\mathbf{k}+1}=(1-k_{1,k+1}){\hat{\mathbf{\varphi }}}_{\mathbf{k}+1}^{-}+k_{1,k+1}{\varphi }_{m,k+1}\\ & {\hat{\mathbf{\theta }}}_{\mathbf{k}+1}=(1-k_{2,k+1}){\hat{\mathbf{\theta }}}_{\mathbf{k}+1}^{-}+k_{2,k+1}{\theta }_{m,k+1}\\ & {\hat{\mathbf{\psi }}}_{\mathbf{k}+1}=(1-k_{3,k+1}){\hat{\mathbf{\psi }}}_{\mathbf{k}+1}^{-}+k_{3,k+1}{\psi }_{m,k+1}\end{array}$$
  六、后验估计误差协方差矩阵
  $$\{\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}=(1-k_{3,k+1})p_{\psi ,k+1}^{-}\end{array}$$

将上述步骤通过C语言实现

• 函数接口说明
  函数输入参数：
  $gx,gy,gz$：陀螺仪角速度数据，单位：$rad/s$；
  $ax,ay,az$：加速度计数据，单位不管；
  $mx,my,mz$：磁力计数据，单位不管；
  注意：要将三组传感器的坐标系对应上，统一方向。

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

float roll = 0,pitch = 0,yaw = 0; 
//euro angle
static float Xk_[3] = {0};  
//Prior estimation
static float Xk[3] = {0};
//Posterior estimation
static float Uk[3] = {0};
//system input
static float Zk[3] = {0};
//measure status
static float Pk[3] = {1,1,1};
//Posteriori estimation error covariance
static float Pk_[3] = {0};
//Prior estimation error covariance
static float K[3] = {0};
//kalman gain
static float Q[3] = {1,1,1};
//System noise covariance
static float R[3] = {1,1,1};
//Measure noise covariance
static float T = 0.002f;
//Discrete time
//float mx_max = FLT_MIN,mx_min = FLT_MAX;
float mx_max = 30.0f,mx_min = -24.9f;
//Maximum and minimum values of magnetometer X-axis
//float my_max = FLT_MIN,my_min = FLT_MAX;
float my_max = 25.2f,my_min = -30.15f;
//Maximum and minimum values of magnetometer Y-axis
//float mz_max = FLT_MIN,mz_min = FLT_MAX;
float mz_max = 33.3f,mz_min = -23.25f;
//Maximum and minimum values of magnetometer Z-axis

//attitude solution by kalman filter
void kalman_filter_attitude_solution_calibyaw(float gx,float gy,float gz,float ax,float ay,float az,float mx,float my,float mz)
{
	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]);
	//step1 - system input
	
	Xk_[0] = Xk[0] + T * Uk[0];
	Xk_[1] = Xk[1] + T * Uk[1];
	Xk_[2] = Xk[2] + T * Uk[2];
	
	if( Xk_[2] > 3.141f ) //if it is greater than PI
	{
		Xk_[2] -= - 3.141f * 2.f;
	}		
	else if( Xk_[2] < -3.141f ) //if it is less than -PI
	{
		Xk_[2] += 3.141f * 2.f;
	}
	
	//step2 - Prior estimation
	
	Pk_[0] = Pk[0] + Q[0];
	Pk_[1] = Pk[1] + Q[1];
	Pk_[2] = Pk[2] + Q[2];
	//step3 - Prior estimation error covariance
	
	K[0] = Pk_[0] / (Pk_[0] + R[0]);
	K[1] = Pk_[1] / (Pk_[1] + R[1]);
	K[2] = Pk_[2] / (Pk_[2] + R[2]);
	//step4 - kalman gain
	
	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;
	//step5 - calculation yaw by magnetometer data
	
	Zk[0] = atan( ay / az );
	Zk[1] = -atan( ax / (sqrt( ay * ay + az * az )));
	Zk[2] = atan2( mZy , mZx );
	//step6 - measure data
	
	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];
	//step6 - Posterior estimation
	
	Pk[0] = (1 - K[0]) * Pk_[0];
	Pk[1] = (1 - K[1]) * Pk_[1];
	Pk[2] = (1 - K[2]) * Pk_[2];
	//step7 - Posteriori estimation error covariance
	
	
	roll = Xk[0] / 3.141f * 180.f;
	pitch = Xk[1] / 3.141f * 180.f;
	yaw = Xk[2] / 3.141f * 180.f;
	//calculate the angle,unit: degree
}

总结

基于卡尔曼滤波器的姿态解算加入磁力计能够通过地球磁场方向标定航向角，与无磁力计的解算相比，有以下区别：

• 无磁力计在每次上电时都会以当前位置为0°航向角，即无论以哪个方向上电，解算的航向角yaw均为0，即一个相对的航向信息。
• 带磁力计的姿态解算，则会在上电后得到一个固定的角度值，即一个绝对的航向信息。

博客中的基于卡尔曼滤波器姿态解算实现代码，更多是为了展示推导信息，因此没有更好的优化的代码实现，读者可以根据实际使用场景来进行优化。
相比于其他的惯性导航姿态解算算法如Mahony姿态解算，基于卡尔曼滤波器计算较为复杂，占用的算力资源较大，使用效率低。`