OpenEdv-开源电子网

 找回密码
 立即注册
正点原子全套STM32/FPGA开发资料,上千讲STM32视频教程,RT1052教程免费下载啦...
查看: 3307|回复: 17
打印 上一主题 下一主题

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

[复制链接]

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
跳转到指定楼层
楼主
发表于 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(";");
*/
}


先去吃饭,吃完回来写具体的,公式打起来太麻烦
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
推荐
 楼主| 发表于 2017-7-31 16:59:13 | 只看该作者
当时做的笔记如下,是和代码对应的,可以对照着看
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 1 反对 0

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
推荐
 楼主| 发表于 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里打出来了,贴图上来
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 1 反对 0

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
2#
 楼主| 发表于 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
Z是测量值,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)是滤波误差方差矩阵。
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 反对

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
3#
 楼主| 发表于 2017-5-9 18:46:17 | 只看该作者
第一行angle+=(gyro_m-q_bias) * dt;
对应式子X(k|k-1) = AX(k-1 | k-1) + BU(k) + W(k)
其中要注意的就是
x不是单纯的角度值
x=| 角度值               |
    | 陀螺仪测量误差 |
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 反对

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
5#
 楼主| 发表于 2017-5-9 19:20:31 | 只看该作者
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 反对

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
6#
 楼主| 发表于 2017-5-9 19:45:08 | 只看该作者

这个式子也是我困扰的,如果不加dt,后面推导就不一样。
但实际上是不应该加的
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 反对

使用道具 举报

56

主题

520

帖子

0

精华

高级会员

Rank: 4

积分
964
金钱
964
注册时间
2014-11-18
在线时间
160 小时
7#
发表于 2017-6-15 16:51:12 | 只看该作者
楼主  ,怎么是C++写的算法啊,看的我很难受。
自己选择的路,成家前走完。
回复 支持 反对

使用道具 举报

1

主题

19

帖子

0

精华

初级会员

Rank: 2

积分
113
金钱
113
注册时间
2016-7-5
在线时间
23 小时
8#
发表于 2017-7-30 21:27:19 | 只看该作者
楼主,c_0是什么含义?
回复 支持 反对

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
9#
 楼主| 发表于 2017-7-31 16:55:43 | 只看该作者
aiyeba 发表于 2017-6-15 16:51
楼主  ,怎么是C++写的算法啊,看的我很难受。

跟C的差别不大呀,里面实现都是C
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 反对

使用道具 举报

34

主题

246

帖子

0

精华

高级会员

Rank: 4

积分
561
金钱
561
注册时间
2014-8-24
在线时间
49 小时
10#
 楼主| 发表于 2017-7-31 16:58:05 | 只看该作者
小爱1314 发表于 2017-7-30 21:27
楼主,c_0是什么含义?

记不太清了,我一会把我当时的笔记拍图发上来
找一份喜欢的工作,这样每天工作的8个小时是快乐的。 找一个喜欢的人,这样每天工作之外的16个小时也是快乐的。
回复 支持 反对

使用道具 举报

0

主题

3

帖子

0

精华

初级会员

Rank: 2

积分
61
金钱
61
注册时间
2017-3-22
在线时间
9 小时
12#
发表于 2017-8-31 11:04:20 | 只看该作者
厉害,赞一个
回复 支持 反对

使用道具 举报

0

主题

125

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
221
金钱
221
注册时间
2017-5-26
在线时间
76 小时
13#
发表于 2017-8-31 14:39:43 来自手机 | 只看该作者
顶,,,
回复 支持 反对

使用道具 举报

0

主题

3

帖子

0

精华

初级会员

Rank: 2

积分
61
金钱
61
注册时间
2017-3-22
在线时间
9 小时
14#
发表于 2017-8-31 15:15:39 | 只看该作者
小爱1314 发表于 2017-7-30 21:27
楼主,c_0是什么含义?

这是个扩展卡尔曼滤波,A是求导得到的,只是求导后跟原来的A一样;c_0是观测方程的雅克比矩阵求导得到的,C = [ d(angle) / d(angle)        d(angle) / d(Q_bias) ] =[1 0] = [c_0  c_1]
回复 支持 反对

使用道具 举报

0

主题

62

帖子

0

精华

初级会员

Rank: 2

积分
111
金钱
111
注册时间
2018-11-29
在线时间
9 小时
15#
发表于 2018-12-15 15:53:16 | 只看该作者
确实复杂
回复 支持 反对

使用道具 举报

1

主题

17

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
406
金钱
406
注册时间
2014-8-18
在线时间
89 小时
16#
发表于 2018-12-24 10:57:28 | 只看该作者
mark!正在用
回复 支持 反对

使用道具 举报

1

主题

19

帖子

0

精华

初级会员

Rank: 2

积分
113
金钱
113
注册时间
2016-7-5
在线时间
23 小时
17#
发表于 2019-3-23 12:14:18 | 只看该作者
不知道,还有没有人在研究这个卡尔曼姿态解算,我最近也在看这个,博主问题,我认为是一个参数,但是为什么乘以dt,过程协方差和解算dt有关???有的话,加个好友970765953
回复 支持 反对

使用道具 举报

19

主题

55

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
1232
金钱
1232
注册时间
2014-4-26
在线时间
88 小时
18#
发表于 2019-5-29 18:02:39 | 只看该作者
tao475824827 发表于 2017-7-31 16:59
当时做的笔记如下,是和代码对应的,可以对照着看

这笔记,好骚
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则



正点原子公众号

QQ|手机版|官方淘宝店|微信公众平台|OpenEdv-开源电子网 ( 粤ICP备12000418号-1 )

GMT+8, 2019-7-2 14:25

Powered by OpenEdv-开源电子网

© 2001-2030 OpenEdv-开源电子网

快速回复 返回顶部 返回列表
三级片电影