使用卡尔曼滤波解算六轴数据

使用卡尔曼滤波解算六轴数据

23-08-26更新：已经修复公式显示错乱/已添加相关源码

github: https://github.com/gxt-kt/kf_6_axis

教程链接： https://www.bilibili.com/video/BV1nh411w7b4/

卡尔曼滤波

卡尔曼滤波是一种常用的状态估计方法，其基本原理是对系统状态进行最优估计。其主要思想是将系统的状态分为先验状态和后验状态，通过将先验状态和观测值进行加权平均，得到最优估计状态。

卡尔曼滤波增益的计算是使后验误差协方差矩阵对角线上元素和（迹）最小（协方差最小）

卡尔曼滤波假设系统状态和测量噪声都服从零均值的高斯分布，也就是正态分布的期望值为0。这是因为卡尔曼滤波是一种线性高斯滤波器，它假设噪声是零均值的高斯白噪声，并且系统状态的转移和测量模型都是线性的。在实际应用中，通常可以通过对干扰的均值进行估计，然后进行补偿处理，从而使得干扰的期望值为零。

系统建模：

由现代控制理论知识可知：
$$\{\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_{k-1}+W_k\\ Z_k& =H{X}_k+V_k\end{array}$$
其中$A$是状态转移矩阵，$B$是输入矩阵，$H$是观测矩阵。

相比现控方程多考虑到噪声干扰。分为以下两个：

• $W_k$ 过程噪声：符合正态分布，期望为0，协方差矩阵为$Q$
• $V_k$ 测量噪声：符合正态分布，期望为0，协方差矩阵为$R$

---

将模型考虑到卡尔曼滤波方程中：
$$\{\begin{array}{rl}{\hat{X}}_k^{-}& =A{\hat{X}}_{k-1}+B{u}_k\\ Z_k& =H{\hat{X}}_k\end{array}$$
${\hat{X}}_k^{-}$为先验估计：根据上一时刻的值估计当前时刻的值

注意到几个点：

• 对系统进行建模出现的噪声$W_k$和$V_k$不会直接出现，而是成为$Q_k$和$R_k$分别代表系统噪声协方差矩阵和观测噪声协方差矩阵
• 这里的$Z_k$成为了观测值（也就是实际计算中的一个输入）

后面的五大公式都是基于这个方程建立的

五大公式：

预测方程两个：
$$\begin{array}{rl}{\hat{x}}_k^{-}& =A{\hat{x}}_{k-1}+B{u}_k\\ P_k^{-}& =A{P}_{k-1}{A}^T+Q\end{array}$$
预测方程和系统的物理模型有关，根据物理模型推算下一时刻的状态和协方差

其中，${\hat{x}}_k^{-}$是先验状态估计，$A$是状态转移矩阵，${\hat{x}}_{k-1}$是后验状态估计，$B$是控制输入矩阵，$u_k$是控制输入向量，$P_k^{-}$是先验误差协方差矩阵，$Q$是系统噪声协方差矩阵。

更新方程三个：

1. 卡尔曼增益计算方程：

$$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$$

$K_k$是卡尔曼增益矩阵，$H$是观测矩阵，$R$是观测噪声协方差矩阵

2. 先验误差协方差矩阵更新方程：

$$P_k=(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T$$

或者简化形式是（两者等价）：
$$P_k=(I-K_{k}H)P_k^{-}$$

3. 状态量更新方程：

$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$$

实例：六轴加速度陀螺仪姿态解算

融合三轴加速度计和三轴角速度计

• 对三轴角速度计进行建模，构建两个预测方程
• 使用三轴加速度计进行观测

---

首先对系统进行建模：

[ r o l l _ g y r o k − p i t c h _ g y r o k − y a w _ g y r o k − ] = [ 1 0 0 0 1 0 0 0 1 ] [ r o l l _ g y r o k − 1 p i t c h _ g y r o k − 1 y a w _ g y r o k − 1 ] + [ Δ t 0 0 0 Δ t 0 0 0 Δ t ] [ g y r o _ x g y r o _ y g y r o _ z ] [ r o l l _ a c c p i t c h _ a c c y a w _ a c c ] = [ 1 0 0 0 1 0 0 0 0 ] [ r o l l _ e s t i k p i t c h _ e s t i k y a w _ e s t i k ] \begin{aligned} & \begin{bmatrix} roll\_gyro_{k}^{-} \\ pitch\_gyro_{k}^{-} \\ yaw\_gyro_{k}^{-}\end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} roll\_gyro_{k-1} \\ pitch\_gyro_{k-1}\\ yaw\_gyro_{k-1}\end{bmatrix}+ \begin{bmatrix} {\Delta}t & 0 & 0 \\ 0 & {\Delta}t & 0 \\ 0 & 0 & {\Delta}t \end{bmatrix} \begin{bmatrix} gyro\_x \\ gyro\_y \\ gyro\_z \end{bmatrix} \\ & \begin{bmatrix} roll\_acc \\ pitch\_acc \\ yaw\_acc\end{bmatrix} =\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0\\ 0 & 0 & 0\end{bmatrix} \begin{bmatrix} roll\_esti_{k} \\ pitch\_esti_{k} \\ yaw\_esti_{k}\end{bmatrix} \end{aligned} ​ ​roll_gyrok−​pitch_gyrok−​yaw_gyrok−​​ ​= ​100​010​001​ ​ ​roll_gyrok−1​pitch_gyrok−1​yaw_gyrok−1​​ ​+ ​Δt00​0Δt0​00Δt​ ​ ​gyro_xgyro_ygyro_z​ ​ ​roll_accpitch_accyaw_acc​ ​= ​100​010​000​ ​ ​roll_estik​pitch_estik​yaw_estik​​ ​​

状态量为 [ r o l l _ g y r o p i t c h _ g y r o y a w _ g y r o ] \begin{bmatrix} roll\_gyro \\ pitch\_gyro\\ yaw\_gyro\end{bmatrix} ​roll_gyropitch_gyroyaw_gyro​ ​，含义直接为角度计算出来的姿态角（只和角速度有关）

状态转移矩阵为 [ 1 0 0 0 1 0 0 0 1 ] \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0& 0& 1\end{bmatrix} ​100​010​001​ ​

输入直接是角速度

$Z_k$为为加速度计求得的姿态角

[ r o l l _ e s t i k p i t c h _ e s t i k y a w _ e s t i k ] \begin{bmatrix} roll\_esti_{k} \\ pitch\_esti_{k} \\ yaw\_esti_{k}\end{bmatrix} ​roll_estik​pitch_estik​yaw_estik​​ ​为由状态量和观测量估计的值

---

五个方程计算：

${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_k$对应的方程为：
[ r o l l _ g y r o k p i t c h _ g y r o k y a w _ g y r o k ] = [ 1 0 0 0 1 0 0 0 1 ] [ r o l l _ g y r o k − 1 p i t c h _ g y r o k − 1 y a w _ g y r o k − 1 ] + [ Δ t 0 0 0 Δ t 0 0 0 Δ t ] [ g y r o _ x g y r o _ y g y r o _ z ] \begin{bmatrix} roll\_gyro_{k} \\ pitch\_gyro_{k}\\ yaw\_gyro_{k}\end{bmatrix} = \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0& 1\end{bmatrix} \begin{bmatrix} roll\_gyro_{k-1} \\ pitch\_gyro_{k-1}\\ yaw\_gyro_{k-1}\end{bmatrix}+ \begin{bmatrix} {\Delta}t & 0 & 0 \\ 0 & {\Delta}t & 0\\ 0 & 0& {\Delta}t\end{bmatrix} \begin{bmatrix} gyro\_x \\ gyro\_y\\ gyro\_z \end{bmatrix} ​roll_gyrok​pitch_gyrok​yaw_gyrok​​ ​= ​100​010​001​ ​ ​roll_gyrok−1​pitch_gyrok−1​yaw_gyrok−1​​ ​+ ​Δt00​0Δt0​00Δt​ ​ ​gyro_xgyro_ygyro_z​ ​

根据建模内容，可以知道：

$A$状态转移矩阵为 [ 1 0 0 0 1 0 0 0 1 ] \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1& 0 \\ 0 & 0& 1\end{bmatrix} ​100​010​001​ ​，$B$= [ Δ t 0 0 0 Δ t 0 0 0 Δ t ] \begin{bmatrix} {\Delta}t & 0 & 0 \\ 0 & {\Delta}t & 0\\ 0 & 0& {\Delta}t\end{bmatrix} ​Δt00​0Δt0​00Δt​ ​，其中$\Delta t$为两次循环时间计算间隔

[ g y r o _ x g y r o _ y g y r o _ z ] \begin{bmatrix} gyro\_x \\ gyro\_y\\ gyro\_z \end{bmatrix} ​gyro_xgyro_ygyro_z​ ​为输入对应轴上的角速度， [ r o l l _ g y r o k p i t c h _ g y r o k y a w _ g y r o k ] \begin{bmatrix} roll\_gyro_{k} \\pitch\_gyro_{k}\\yaw\_gyro_{k}\end{bmatrix} ​roll_gyrok​pitch_gyrok​yaw_gyrok​​ ​为由角速度计算出的状态量

---

$P_k^{-}=A{P}_{k-1}{A}^T+Q$对应的方程为：
P k − = [ 1 0 0 0 1 0 0 0 1 ] P k − 1 [ 1 0 0 0 1 0 0 0 1 ] + Q P_{k}^{-}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1& 0 \\ 0 & 0& 1\end{bmatrix} P_{k-1} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1& 0 \\ 0 & 0& 1\end{bmatrix} + Q Pk−​= ​100​010​001​ ​Pk−1​ ​100​010​001​ ​+Q

---

$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$对应的方程为：
K k = P k − [ 1 0 0 0 1 0 0 0 0 ] ( [ 1 0 0 0 1 0 0 0 0 ] P k − [ 1 0 0 0 1 0 0 0 0 ] + R ) − 1 = P k − ( P k − + R ) − 1 \begin{aligned} K_{k}&=P_{k}^{-}\begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0& 0\end{bmatrix} ( \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0& 0\end{bmatrix}P_{k}^{-} \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0& 0\end{bmatrix}+R )^{-1} \\ &=P_{k}^{-}(P_{k}^{-}+R)^{-1} \end{aligned} Kk​​=Pk−​ ​100​010​000​ ​( ​100​010​000​ ​Pk−​ ​100​010​000​ ​+R)−1=Pk−​(Pk−​+R)−1​

---

$P_k=(I-K_{k}H)P_k^{-}$对应的方程为：

P k = ( [ 1 0 0 0 1 0 0 0 1 ] − K k [ 1 0 0 0 1 0 0 0 0 ] ) P k − \begin{aligned} P_{k} &= ( \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0& 1\end{bmatrix}- K_k \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0 \\ 0 & 0& 0\end{bmatrix} ) P_{k}^{-} \end{aligned} Pk​​=( ​100​010​001​ ​−Kk​ ​100​010​000​ ​)Pk−​​

---

${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$对应的方程为：

x ^ k = x ^ k − + K k ( [ r o l l _ a c c p i t c h _ a c c y a w _ a c c ] − [ 1 0 0 0 1 0 0 0 0 ] x ^ k − ) \hat{x}_{k} = \hat{x}_{k}^{-} + K_k( \begin{bmatrix} roll\_acc \\ pitch\_acc\\ yaw\_acc\end{bmatrix}- \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0& 0 & 0\end{bmatrix} \hat{x}_{k}^{-}) x^k​=x^k−​+Kk​( ​roll_accpitch_accyaw_acc​ ​− ​100​010​000​ ​x^k−​)

---

初始化值

Q = [ 0.002 0 0 0 0.002 0 0 0 0.002 ] Q=\begin{bmatrix} 0.002 & 0 & 0 \\ 0 & 0.002 & 0 \\ 0 & 0 & 0.002 \end{bmatrix} Q= ​0.00200​00.0020​000.002​ ​

R = [ 0.2 0 0 0 0.2 0 0 0 0.2 ] R=\begin{bmatrix} 0.2 & 0 & 0 \\ 0 & 0.2 & 0 \\ 0 & 0 & 0.2 \end{bmatrix} R= ​0.200​00.20​000.2​ ​

P 0 = [ 1 0 0 0 1 0 0 0 1 ] P_{0}=\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1\end{bmatrix} P0​= ​100​010​001​ ​

x 0 = [ 0 0 0 ] x_{0}=\begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} x0​= ​000​ ​

---

示例源码：

#pragma once

#include 
#include 
#include 

// @input:
//   time: time stamp (s)
//   acc_*: accelerate (m/s^2) actually the unit is not important
//   gyro_*: gyroscope (rad/s)
// @Output:
//   Eigen::Vector3 : roll pitch yaw (rad/s)
template <typename Time, typename T>
inline Eigen::Vector3<T> KF_6_Axis(Time time, T acc_x, T acc_y, T acc_z,
                                   T gyro_x, T gyro_y, T gyro_z) {
  static Eigen::Matrix<T, 3, 3> Q = decltype(Q)::Identity();
  Q << 0.002, 0, 0, 0, 0.002, 0, 0, 0, 0.002;
  static Eigen::Matrix<T, 3, 3> R = decltype(R)::Identity();
  R << 0.2, 0, 0, 0, 0.2, 0, 0, 0, 0.2;

  static Eigen::Matrix<T, 3, 3> A = decltype(A)::Identity();
  static Eigen::Matrix<T, 3, 3> B = decltype(B)::Identity();
  static Eigen::Matrix<T, 3, 3> H = decltype(H)::Identity();
  H << 1, 0, 0, 0, 1, 0, 0, 0, 0;

  static Eigen::Vector3<T> X_bar_k_1 = decltype(X_bar_k_1)::Zero();
  static Eigen::Vector3<T> u_k = decltype(u_k)::Zero();
  static Eigen::Vector3<T> X_bar_k__ = decltype(X_bar_k__)::Zero();
  static Eigen::Vector3<T> X_bar_k = decltype(X_bar_k)::Zero();
  static Eigen::Vector3<T> Z_k = decltype(Z_k)::Zero();
  static Eigen::Matrix<T, 3, 3> P_k_1 = decltype(P_k_1)::Zero();
  static Eigen::Matrix<T, 3, 3> P_k__ = decltype(P_k__)::Zero();
  static Eigen::Matrix<T, 3, 3> P_k = decltype(P_k)::Identity();
  static Eigen::Matrix<T, 3, 3> K_k = decltype(K_k)::Zero();
  static Eigen::Matrix<T, 3, 3> I = decltype(I)::Identity();

  /*计算微分时间*/
  static Time last_time;           // 上一次采样时间(s)
  double dt = (time - last_time);  // 微分时间(s)
  last_time = time;

  B = decltype(B)::Identity() * dt;
  /*计算x,y轴上的角速度*/
  T droll_dt =
      gyro_x +
      ((sin(X_bar_k(1)) * sin(X_bar_k(0))) / cos(X_bar_k(1))) * gyro_y +
      ((sin(X_bar_k(1)) * cos(X_bar_k(0))) / cos(X_bar_k(1))) *
          gyro_z;  // roll轴的角速度
  T dpitch_dt =
      cos(X_bar_k(0)) * gyro_y - sin(X_bar_k(0)) * gyro_z;  // pitch轴的角速度
  T dyaw_dt = sin(X_bar_k(0)) / cos(X_bar_k(1)) * gyro_y +
              cos(X_bar_k(0)) / cos(X_bar_k(1)) * gyro_z;

  u_k(0) = droll_dt;
  u_k(1) = dpitch_dt;
  u_k(2) = dyaw_dt;

  // 第一步计算 状态外推方程
  X_bar_k__ = A * X_bar_k_1 + B * u_k;
  // 第二步计算 协方差外推方程
  P_k__ = A * P_k_1 * A.transpose() + Q;
  // 第三步计算 卡尔曼增益
  K_k = P_k__ * H.transpose() * (H * P_k__ * H.transpose() + R).inverse();
  // 第四步计算 更新协方差矩阵
  P_k = (I - K_k * H) * P_k__;

  // 下面计算观测量
  // roll角度
  T acc_roll = atan(acc_y / acc_z);
  // pitch角度
  T acc_pitch = -1 * atan(acc_x / sqrt(pow(acc_y, 2) + pow(acc_z, 2)));
  // 对观测矩阵赋值
  Z_k(0) = acc_roll;
  Z_k(1) = acc_pitch;
  Z_k(2) = 0;

  // 第五步计算 更新状态量
  X_bar_k = X_bar_k__ + K_k * (Z_k - H * X_bar_k__);

  // 更改历史值为下次循环做准备
  P_k_1 = P_k;
  X_bar_k_1 = X_bar_k;

  return {X_bar_k(0), X_bar_k(1), X_bar_k(2)};
}