智能车制作

标题: 卡尔曼滤波算法相关问题 [打印本页]

作者: dyl0000 时间: 2013-8-2 21:35
标题: 卡尔曼滤波算法相关问题
请教各位大神，下面这段卡尔曼滤波程序中，协方差矩阵以及其更新是怎么计算的，写成公式是怎么样的？还有我只知道不做融合（单独一个陀螺仪数据）时卡尔曼增益和协方差的关系，做了融合后，两者之间的关系又变成什么样了，表示线代太差看不懂。。。还有R_angle 和 Q_angle是不是就是卡尔曼滤波理论中的测量协方差和预测协方差（陀螺仪的），也就是说R_angle 越小测量值的可信度越高？Q_gyro是什么，如果是加速度计的测量协方差，那加速度计怎么没有预测协方差？
求哪位好心人指点，我下了好多资料，包括卡尔曼的论文都仔细看了，还看了好多论坛的帖子，就是没找到答案，当然也可能是没看懂忽略了，好像网上的论文要么泛泛而谈，要么直接给出那5个式子，要么就太难。

刚加入这个论坛不久，看到好像有好多高人，请各位高人指点，我在这里拜谢了！！！

//float gyro_m：陀螺仪测得的量（角速度）

//float incAngle:加计测得的角度值

#define dt 0.02//卡尔曼滤波采样频率

#define R_angle 0.5 //测量噪声的协方差（即是测量偏差）

#define Q_angle 0.0001//过程噪声的协方差

#define Q_gyro 0.0003 //过程噪声的协方差过程噪声协方差为一个一行两列矩阵

float kalmanUpdate(const float gyro_m,constfloat incAngle)

{

float K_0;//含有卡尔曼增益的另外一个函数，用于计算最优估计值

float K_1;//含有卡尔曼增益的函数，用于计算最优估计值的偏差

float Y_0;

float Y_1;

float Rate;//去除偏差后的角速度

float Pdot[4];//过程协方差矩阵的微分矩阵

float angle_err;//角度偏量

float E;//计算的过程量

static float angle = 0; //下时刻最优估计值角度

static float q_bias = 0; //陀螺仪的偏差

static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵 |1 0|

|0 1|

//计算过程协方差矩阵的微分矩阵

Pdot[0] = Q_angle - P[0][1] - P[1][0];//？？？？？？ | 0.0001 - 1 |

Pdot[1] = - P[1][1]; // |- 1 0.0003 |

Pdot[2] = - P[1][1];

Pdot[3] = Q_gyro;//？？？？？？

angle += Rate * dt; //角速度积分得出角度

P[0][0] += Pdot[0] * dt; //计算协方差矩阵 | 1.00002 -0.02 |

P[0][1] += Pdot[1] * dt; // |- 0.02 1.00006 |

P[1][0] += Pdot[2] * dt;

P[1][1] += Pdot[3] * dt;

Rate = gyro_m - q_bias;// 去除偏差后的角速度

angle_err = incAngle - angle; //计算角度偏差

E = R_angle + P[0][0];

K_0 = P[0][0] / E; //计算卡尔曼增益

K_1 = P[1][0] / E;

Y_0 = P[0][0];

Y_1 = P[0][1];

P[0][0] -= K_0 * Y_0; //跟新协方差矩阵

P[0][1] -= K_0 * Y_1;

P[1][0] -= K_1 * Y_0;

P[1][1] -= K_1 * Y_1;

angle += K_0 * angle_err; //给出最优估计值

q_bias += K_1 * angle_err;//跟新最优估计值偏差

return angle;

}

作者: dyl0000 时间: 2013-8-2 21:39
顺便一提，这个程序已经经过验证，是可以用的，在找卡尔曼滤波程序的基友们可以直接用

作者: A楼飘房 时间: 2013-8-3 07:51
谢谢分享，学习了。

作者: 于山boy 时间: 2013-8-3 15:23
摄像头的飘过

作者: nimanima 时间: 2013-8-3 15:39
先去找到系统的状态空间模型

作者: gy810986741 时间: 2013-8-3 15:49
有关卡尔曼滤波可以去阿莫论坛上面找找，那里讲的比较详细

作者: dyl0000 时间: 2013-8-3 16:15

gy810986741 发表于 2013-8-3 15:49
有关卡尔曼滤波可以去阿莫论坛上面找找，那里讲的比较详细

感谢你回复我，那个论坛上的确有很多关于卡尔曼滤波的，但是关键的二维滤波中用线性代数求解方程的方法还是没有，有人说他自己推了，就是没有人拿出来分享，我只能等待有好心人不怕麻烦回复我了

作者: dyl0000 时间: 2013-8-3 16:24

nimanima 发表于 2013-8-3 15:39
先去找到系统的状态空间模型

这是我在阿莫电子论坛上看到的：论文里的模型的特点，首先模型是一个2维模型，故在写程序的时候需要先把卡尔曼滤波的五个方程用线性代数的方法解算出来，这样才能够进行相应的卡尔曼滤波程序的编写。贴上程序的文档（一定注意这是2维的卡尔曼滤波，根据论文中的五个方程进行相应的线性代数化简才能够看懂程序），一定注意程序是在用线性代数解算后的方程
然后5个方程如下：

卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。

现设线性时变系统的离散状态防城和观测方程为：

X(k) =F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) =H(k)·X(k)+N(k)

其中，X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

F(k,k-1)为状态转移矩阵

U(k)为k时刻动态噪声

T(k,k-1)为系统控制矩阵

H(k)为k时刻观测矩阵

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为：

预估计X(k)^=F(k,k-1)·X(k-1)

计算预估计协方差矩阵
C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
Q(k) = U(k)×U(k)'
计算卡尔曼增益矩阵
K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
R(k) = N(k)×N(k)'
更新估计
X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]
计算更新后估计协防差矩阵
C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'
X(k+1) = X(k)~
C(k+1) = C(k)~
重复以上步骤

模型是有，可我就是不会化简怎么办

作者: q1552811728 时间: 2013-8-8 10:20
楼主，个人以为程序有一些问题，例如static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵    |1  0|
                                                      |0 1|
   //计算过程协方差矩阵的微分矩阵应该在程序之前初始化，因为你如果放在程序里，每次调用都要初始化  P[2][2]，而 P[2][2]应该是一个累积量，你每次都初始化，就没有累积的效果了，我之前也是这样的，但感觉有点不对，所以改了。之所以那个程序能用，是因为如果你将公式带入简化的话，你会发现这是一个Tz可微变的清华方案的滤波。所以程序自然可以用

作者: q1552811728 时间: 2013-8-8 10:22
我也在学习卡尔曼，观点可能不对，希望大家可以交流一下

作者: dyl0000 时间: 2013-8-15 12:46

q1552811728 发表于 2013-8-8 10:22
我也在学习卡尔曼，观点可能不对，希望大家可以交流一下

。。。关键是我根本看不懂程序里面的矩阵是怎么来的

，请问你说的清华的那个滤波是怎么回事？可以分享下吗？

作者: dyl0000 时间: 2013-8-15 12:58

q1552811728 发表于 2013-8-8 10:22
我也在学习卡尔曼，观点可能不对，希望大家可以交流一下

[attach]49808[/attach]这个里面好像有说：Q,R 虽然都是关于时间的变量，但是由于卡尔曼滤波有很好的收敛性，所以可以将Q，R 都取比较极端的参数。用常量来定义。比如
static float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};
static float Pdot[4] ={0,0,0,0};
不过我还是不能很好的理解，期待更多交流

作者: 快乐之神11 时间: 2013-8-18 10:00
四轴一个很好玩的东西很好

作者: 梦小孩 时间: 2013-8-26 09:42
滤波收敛效果不理想，该怎么改进呢？

作者: pangfumin 时间: 2013-11-17 13:26
  //计算过程协方差矩阵的微分矩阵
   Pdot[0] = Q_angle - P[0][1] - P[1][0];//？？？？？？       | 0.0001    - 1 |
   Pdot[1] = - P[1][1];             //                   |- 1    0.0003 |
   Pdot[2] = - P[1][1];
   Pdot[3] = Q_gyro;//？？？？？？
   angle += Rate * dt; //角速度积分得出角度
   P[0][0] += Pdot[0] * dt; //计算协方差矩阵          | 1.00002    -0.02 |
   P[0][1] += Pdot[1] * dt; //                         |- 0.02    1.00006 |
   P[1][0] += Pdot[2] * dt;
   P[1][1] += Pdot[3] * dt;

这段程序是对协方差预测的简化吧，不知道为什么这么简化

作者: wumin11413 时间: 2013-12-3 20:36
#define dt                0.02//卡尔曼滤波采样频率

#define R_angle       0.5 //测量噪声的协方差（即是测量偏差）

#define Q_angle       0.0001//过程噪声的协方差

#define Q_gyro          0.0003 //过程噪声的协方差
这几个值怎么确定？是都要调吗？

作者: 涛子 时间: 2013-12-9 18:24

作者: hotsauce1861 时间: 2014-3-13 21:33
今天有点累蒙了，我也在学习卡尔曼，楼主可以多多交流交流呢

作者: Complicated 时间: 2014-3-16 10:04
楼主，我们在AD采集数据后怎么才能转换成角度啊？你是怎么做到？我们一点头绪都没有。

作者: 凝风炫魂 时间: 2014-4-10 16:15

q1552811728 发表于 2013-8-8 10:20
楼主，个人以为程序有一些问题，例如static float P[2][2] = {{ 1, 0 }, { 0, 1 }};//过程协方差矩阵 ...

你说错了喔，static是静态局部变量，跟全局变量的意思一样的，不会每次初始化的喔

作者: 智能时代 时间: 2014-7-14 16:05
请问楼主，变量：incAngle:加计测得的角度值，，是加速度计的哪个轴呢？Z还是X? ....还有就是R_angle 。Q_angle。Q_gyro三个参数对每个直立是否要更改？

作者: jiushibuzhidao 时间: 2014-7-29 10:21

dyl0000 发表于 2013-8-2 21:39
顺便一提，这个程序已经经过验证，是可以用的，在找卡尔曼滤波程序的基友们可以直接用

楼主好！我用卡尔曼滤波时，滤波后的波形跟滤波前的波形比变化慢，就是我的模块动了过后要慢慢变化到模块的东西的那个位置，滤波前的波形就能很快反应到那个位置，我把Q_angle这个参数改大了快接近1了就解决了这个问题，请问这样行吗？

作者: 葫芦导弹 时间: 2016-7-13 09:03
合工大握手······挖坟抱歉了哈哈，师兄你的程序第一句是不是应该移到这段程序最后啊，RATE没赋初值啊，还有卡尔曼响应太慢是怎么回事，修改之后还是慢

   angle += Rate * dt; //角速度积分得出角度
   P[0][0] += Pdot[0] * dt; //计算协方差矩阵          | 1.00002    -0.02 |
   P[0][1] += Pdot[1] * dt; //                         |- 0.02    1.00006 |
   P[1][0] += Pdot[2] * dt;
   P[1][1] += Pdot[3] * dt;
   Rate = gyro_m - q_bias;// 去除偏差后的角速度

欢迎光临智能车制作 (http://dns.znczz.com/)