卡尔曼滤波器程序学习经验分享！

最近两日在看卡尔曼滤波。

最近两日在看卡尔曼滤波。
看完理论后，从网上搜索了一段代码来看，然后就懵逼了。
这段代码被人转的次数特别多，应该做飞控的都用过。
不知道各位有没有自己推导过。
反正我是中间有一步不太懂。
在这里先把自己弄懂的给大伙分享一下，也是花了两天才搞懂的，应该会对一些新手有帮助的。
不懂的地方，麻烦用过的大神帮忙给个思路。
谢谢！
先上代码吧
/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/
#ifndef KalmanFilter_h
#define KalmanFilter_h
#include
class KalmanFilter
{
  public:
  KalmanFilter();

  /*
卡尔曼融合计算
angle_m: 加速度计测量并通过atan2(ax,ay)方法计算得到的角度（弧度值）
gyro_m:陀螺仪测量的角速度值（弧度值）
dt:采样时间(s)
outAngle:卡尔曼融合计算出的角度（弧度值）
outAngleDot:卡尔曼融合计算出的角速度（弧度值）
  */
  void getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot);

  private:
  double C_0, Q_angle, Q_gyro, R_angle;
  double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
  double angle, angle_dot;
  double P[2][2];
  double Pdot[4];
};
/*CPP文件  */
/*
* KalmanFilter.cpp
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/
#include "KalmanFilter.h"
KalmanFilter::KalmanFilter()
{
C_0 = 1.0f;
Q_angle = 0.001f;//加速度计的过程噪声协方差
Q_gyro = 0.003f;//陀螺仪的过程噪声协方差
R_angle = 0.5f;//加速度计的测量噪声协方差
q_bias = angle_err = PCt_0 = PCt_1 = E = K_0 = K_1 = t_0 = t_1 = 0.0f;
angle = angle_dot = 0.0f;
P[0][0] = 1.0f;
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 1.0f;
Pdot[0] = 0.0f;
Pdot[1] = 0.0f;
Pdot[2] = 0.0f;
Pdot[3] = 0.0f;
}
void  KalmanFilter::getValue(double angle_m, double gyro_m, double dt, double &outAngle, double &outAngleDot)
{
/*
  Serial.print("angle_m = ");
  Serial.print(angle_m);
  Serial.print(";");
  Serial.print("gyro_m = ");
  Serial.print(gyro_m);
  Serial.print(";");
*/
  //陀螺仪积分的先验角度
  angle+=(gyro_m-q_bias) * dt;
  //角度偏差(用于计算后验状态估计)
  angle_err = angle_m - angle;
  //先验误差协方差的微分
  Pdot[0] = Q_angle - P[0][1] - P[1][0];
  Pdot[1] = -P[1][1];
  Pdot[2] = -P[1][1];
  Pdot[3] = Q_gyro;
  //先验误差协方差的积分
  P[0][0] += Pdot[0] * dt;//计算加速度计过程噪声协方差
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;//计算陀螺仪过程噪声协方差
  //卡尔曼增益计算
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  E = R_angle + C_0 * PCt_0;//测量余量协方差
  K_0 = PCt_0 / E;//加速度计的卡尔曼增益
  K_1 = PCt_1 / E;//陀螺仪偏差的卡尔曼增益
  //后验误差协方差计算
  t_0 = PCt_0;
  t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  //更新状态
  angle += K_0 * angle_err;//后验估计最优角度值
  q_bias += K_1 * angle_err;//更新最优估计值偏差
  angle_dot = gyro_m-q_bias;//更新最优角速度(后验估计)

  outAngle = angle;
  outAngleDot = angle_dot;
/*
  Serial.print("angle = ");
  Serial.print(angle);
  Serial.print(";");
  Serial.print("angle_dot = ");
  Serial.print(angle_dot);
  Serial.print(";");
*/
}
先去吃饭，吃完回来写具体的，公式打起来太麻烦

陆兰兰 · 2019-8-5 06:36:33

当时做的笔记如下，是和代码对应的，可以对照着看

康永清 · 2019-8-5 06:53:53

第二行
//先验误差协方差的微分
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = -P[1][1];
Pdot[2] = -P[1][1];
Pdot[3] = Q_gyro;
//先验误差协方差的积分
P[0][0] += Pdot[0] * dt;//计算加速度计过程噪声协方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;//计算陀螺仪过程噪声协方差
这些对应式子
P(k|k-1) = AP(k-1|k-1)A' + Q(k)
这也是这段程序比较难理解的地方
这个究竟是怎么推导出来的，
word里打出来了，贴图上来

h1654155275.5684 · 2019-8-5 07:09:42

第二给出公式，具体怎么和程序对应的，一会下面说
公式1：X(k|k-1) = AX(k-1 | k-1) + BU(k) + W(k)公式2：P(k|k-1) = AP(k-1|k-1)A' + Q(k)公式3：Kg(k) = P(k|k-1)H'/{HP(k|k-1)H' + R} //卡尔曼增益公式4：X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)公式5：P(k|k) = (1- Kg(k) H) P(k|k-1)另外，Z(k) = HX(k) + VZ是测量值，X是系统值，W是过程噪声，V是测量噪声，H是测量矩阵，A是转移矩阵，Q是W的协方差，R是V的协方差，X(k|k-1)是估计值；X(k|k)是X(k|k-1)的最优估计值，即滤波估计值；P(k|k-1)是估计值误差方差矩阵，P(k|k)是滤波误差方差矩阵。