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

---

---

前言

上一篇博客对基于卡尔曼滤波器的姿态解算进行了一定程度的公式推导，这篇博客将进行算法公式的代码实现，并对姿态解算效果进行效果演示。
考虑到有一些单片机上无法直接进行矩阵运算，因此本篇博客的主要内容之一就是将矩阵运算拆解成方程组形式。

公式解析

• $step1$：获取欧拉角的导数
  $$\begin{array}{l}\left[\begin{array}{c}\dot{\mathbf{\varphi }}\\ \dot{\mathbf{\theta }}\\ \dot{\mathbf{\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]\end{array}$$
  其中，$\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$是陀螺仪角速度数据。
  整理成离散形式的方程组为：
  $$\{\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}\text{\hspace{1em}(1)}$$

• $step2$：先验估计
  $$\left[\begin{array}{c}{\hat{\mathbf{\varphi }}}_{\mathbf{k}+1}^{-}\\ {\hat{\mathbf{\theta }}}_{\mathbf{k}+1}^{-}\\ {\hat{\mathbf{\psi }}}_{\mathbf{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{\mathbf{\varphi }}}_{\mathbf{k}}\\ {\hat{\mathbf{\theta }}}_{\mathbf{k}}\\ {\hat{\mathbf{\psi }}}_{\mathbf{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{{\mathbf{\varphi }}_{\mathbf{k}}}\\ \dot{{\mathbf{\theta }}_{\mathbf{k}}}\\ \dot{{\mathbf{\psi }}_{\mathbf{k}}}\end{array}\right]$$
  整理成离散形式的方程组
  $$\{\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}\text{\hspace{1em}(2)}$$

• $step3$：先验估计误差协方差矩阵（矩阵A是单位矩阵）
  $$P_{k+1}^{-}=A{P}_k{A}^T+Q$$
  令系统噪声协方差矩阵$Q=\left[\begin{array}{ccc}{q}_{\varphi }& 0& 0\\ 0& q_{\theta }& 0\\ 0& 0& q_{\psi }\end{array}\right]$，先验估计误差协方差矩阵为$P_k^{-}=\left[\begin{array}{ccc}{p}_{\varphi ,k}^{-}& 0& 0\\ 0& p_{\theta ,k}^{-}& 0\\ 0& 0& p_{\psi ,k}^{-}\end{array}\right]$，后验估计误差协方差矩阵为$P_k=\left[\begin{array}{ccc}{p}_{\varphi ,k}& 0& 0\\ 0& p_{\theta ,k}& 0\\ 0& 0& p_{\psi ,k}\end{array}\right]$，则得到
  $$\{\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)}$$

• $step4$：卡尔曼增益矩阵计算（矩阵除法即乘以矩阵的逆）
  $$K_{k+1}=\frac{P_{k+1}^{-}{H}^T}{H{P}_{k+1}^{-}{H}^T+R}$$
  其中，$H=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]$，令测量噪声协方差矩阵$R=\left[\begin{array}{ccc}{r}_{\varphi }& 0& 0\\ 0& r_{\theta }& 0\\ 0& 0& r_{\psi }\end{array}\right]$，令卡尔曼增益矩阵$K_k=\left[\begin{array}{ccc}{k}_{1,k}& 0& 0\\ 0& k_{2,k}& 0\\ 0& 0& k_{3,k}\end{array}\right]$，则得到：
  $$\{\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)}$$

• $step5$：获取欧拉角测量值
  令$z_k=\left[\begin{array}{c}{\varphi }_{m,k}\\ {\theta }_{m,k}\\ {\psi }_{m,k}\end{array}\right]$，则得到：
  $$\{\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}=0\end{array}\text{\hspace{1em}(5)}$$
  其中，$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$是三轴加速度计数据。

• $step6$：后验估计计算
  $${\hat{x}}_{k+1}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-H{\hat{x}}_{k+1}^{-})$$
  令后验估计状态为$\left[\begin{array}{c}{\hat{\mathbf{\varphi }}}_{\mathbf{k}+1}\\ {\hat{\mathbf{\theta }}}_{\mathbf{k}+1}\\ {\hat{\mathbf{\psi }}}_{\mathbf{k}+1}\end{array}\right]$，可得到：
  $$\{\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}={\hat{\mathbf{\psi }}}_{\mathbf{k}+1}^{-}\end{array}\text{\hspace{1em}(6)}$$
  从这组方程里可以看出航向角漂移问题的来源。

• $step7$：后验估计误差协方差矩阵
  $$P_{k+1}=(I-K_{k+1}H)P_{k+1}^{-}$$
  可以得到：
  $$\{\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$：陀螺仪角速度数据，单位：$rad/s$；
  $ax,ay,az$：加速度计数据，单位不管；
  要将六轴传感器中的角速度坐标系和加速度坐标系对应上，统一方向。

#include <math.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

//attitude solution by kalman filter
void kalman_filter_attitude_solution(float gx,float gy,float gz,float ax,float ay,float az)
{
	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];
	//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] = 0;
	//step4 - kalman gain
	
	Zk[0] = atan( ay / az );
	Zk[1] = -atan( ax / (sqrt( ay * ay + az * az )));
	Zk[2] = 0;
	//step5 - 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] = Xk_[2];
	//step6 - Posterior estimation
	
	Pk[0] = (1 - K[0]) * Pk_[0];
	Pk[1] = (1 - K[1]) * Pk_[1];
	Pk[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
}

效果演示

本博客采用匿名飞控板 + 匿名上位机来展示姿态解算效果不是广告，刚好手上有一块。

姿态解算

总结

• 基于卡尔曼滤波器的姿态解算能够获得准确的姿态角，但是由于测量数据无法对航向角yaw进行校正，导致航向角在一定时间累积误差后会出现漂移问题。这种漂移问题在振动大，电磁干扰大的环境下将更为明显。
• 可以通过初始化时计算平均偏移量来做一定的偏置补偿，但仅能减缓漂移的速度，无法完全抑制。
• 下一期博客的内容，将针对航向角漂移问题，引入磁力计传感来解决。
  若有需要源代码工程可私信我