卡尔曼滤波器融合六轴IMU数据

zhang_xiaoqiang

已于 2023-10-18 15:30:14 修改

阅读量1.8k

点赞数 3

分类专栏：卡尔曼文章标签：算法

于 2023-10-18 15:25:03 首次发布

本文链接：https://blog.csdn.net/zhang_xiaoqiang/article/details/133902911

版权

卡尔曼专栏收录该内容

2 篇文章 0 订阅

订阅专栏

卡尔曼滤波

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

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

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

系统建模

由现代控制理论知识可知：

其中 $A$ 是状态转移矩阵， $B$ 是输入矩阵， $H$ 是观测矩阵。

$W_{k}$ 过程噪声，符合正态分布，期望为0，协方差矩阵为 $Q$

$V_{k}$ 测量噪声，符合正态分布，期望为0，协方差矩阵为 $R$

将模型考虑到卡尔曼滤波方程中：

$\hat{X{_{k}^{-}}}$ 为系统状态的先验估计，根据上一时刻的估计值计算。

注意：

系统建模的噪声 $W_{k}$ 和 $V_{k}$ 不会直接出现，而是 $Q$ 和 $R$ 分别代表系统噪声协方差矩阵和观测噪声协方差矩阵。
这里的 $Z_{k}$ 成为了观测值（就是实际计算中的一个输入，会与预测系统状态做比较）。

五大公式：

预测方程2个

$\hat{X{_{k}^{-}}}$ 是先验状态估计， $A$ 是状态转移矩阵， $B$ 是输入矩阵， $\hat{X_{k-1}}$ 是上一时刻状态估计， $P_{k}^{-}$ 是先验误差协方差矩阵， $Q$ 是系统噪声协方差矩阵。

更新方程3个

卡尔曼增益方程：
误差协方差矩阵更新方程：

或者等价简化为如下公式：

状态量更新方程：

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

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

对3轴角速度建模，构建2个预测方程。
对3轴加速度进行观测

系统建模

状态量 $\begin{bmatrix} rool_-gyro \\ pitch_-gyro \\yaw_-gyro \end{bmatrix}$ 是由角速度计算出的姿态角。

状态转移矩阵 $\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}$ ，输入是角速度 $\begin{bmatrix} gyro_-x \\ gyro_-y \\gyro_-z \end{bmatrix}$

$\begin{bmatrix} roll_-acc \\ pitch_-acc \\ yaw_-acc \end{bmatrix}$ 为 加速度计算的姿态角，对应 $Z_{k}$ 观测值

$\begin{bmatrix} roll_-esti_k \\ pitch_-esti_k \\ yaw_-esti_k \end{bmatrix}$ 为先验估计状态量和观测量计算得到，对应 $\hat{X_{k}}$ 估计值

五个方程的计算

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

$A = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}$ ， $B = \begin{bmatrix} \Delta t & 0 & 0 \\ 0 & \Delta t & 0\\ 0 & 0 & \Delta t \end{bmatrix}$ ,其中 $\Delta t$ 为两次循环时间计算间隔。

$u_k = \begin{bmatrix} gyro_-x \\ gyro_-y \\gyro_-z \end{bmatrix}$ 为3轴角速度, $\hat{x_k^-} = \begin{bmatrix} rool_-gyro_k^- \\ pitch_-gyro_k^- \\yaw_-gyro_k^- \end{bmatrix}$ 为先验状态估计

初始化值

代码：源代码

#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>

// @input:
//   time: time stamp (s)
//   acc_*: accelerate (m/s^2) actually the unit is not important
//   gyro_*: gyroscope (rad/s)
// @Output:
//   Eigen::Vector3<T> : 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)};
}