关于卡尔曼滤波器程序的一些学习总结

tao475824827 · 发表于 2017-5-9 17:25:12

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

/*
* KalmanFilter.h
* Non-original
* Author: x2d
* Copyright (c) 2012 China
*
*/
#ifndef KalmanFilter_h
#define KalmanFilter_h
#include <WProgram.h>
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(";");
*/
}

先去吃饭，吃完回来写具体的，公式打起来太麻烦

tao475824827 · 发表于 2017-7-31 16:59:13

当时做的笔记如下，是和代码对应的，可以对照着看
微信图片_20170731165753.jpg

tao475824827 · 发表于 2017-5-9 17:27:40

第二给出公式，具体怎么和程序对应的，一会下面说

公式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) + V

tao475824827 · 发表于 2017-5-9 18:48:04

第二行
//先验误差协方差的微分
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里打出来了，贴图上来