卡尔曼滤波

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

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

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

系统建模

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

其中是状态转移矩阵，是输入矩阵，是观测矩阵。

过程噪声，符合正态分布，期望为0，协方差矩阵为

测量噪声，符合正态分布，期望为0，协方差矩阵为

---

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

为系统状态的先验估计，根据上一时刻的估计值计算。

注意：

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

五大公式：

预测方程2个

 是先验状态估计，是状态转移矩阵，是输入矩阵，是上一时刻状态估计，是先验误差协方差矩阵，是系统噪声协方差矩阵。

更新方程3个

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

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

1. 状态量更新方程：

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

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

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

系统建模

状态量是由角速度计算出的姿态角 。

状态转移矩阵，输入是角速度

为 加速度计算的姿态角，对应观测值

为先验估计状态量和观测量计算得到，对应估计值

五个方程的计算

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

，,其中为两次循环时间计算间隔。

 为3轴角速度,为先验状态估计

---

---

---

---

---

初始化值

 

 

---

代码：源代码

#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)};
}