卡尔曼滤波算法相关问题

dyl0000

请教各位大神，下面这段卡尔曼滤波程序中，协方差矩阵以及其更新是怎么计算的，写成公式是怎么样的？还有我只知道不做融合（单独一个陀螺仪数据）时卡尔曼增益和协方差的关系，做了融合后，两者之间的关系又变成什么样了，表示线代太差看不懂。。。还有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;

}

葫芦导弹

合工大握手······挖坟抱歉了哈哈，师兄你的程序第一句是不是应该移到这段程序最后啊，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;// 去除偏差后的角速度

jiushibuzhidao

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

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

智能时代

请问楼主，变量：incAngle:加计测得的角度值，，是加速度计的哪个轴呢？Z还是X? ....还有就是R_angle 。Q_angle。Q_gyro三个参数对每个直立是否要更改？

凝风炫魂

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

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

Complicated

楼主，我们在AD采集数据后怎么才能转换成角度啊？你是怎么做到？我们一点头绪都没有。

hotsauce1861

今天有点累蒙了，我也在学习卡尔曼，楼主可以多多交流交流呢

涛子

wumin11413

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

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

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

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

pangfumin

  //计算过程协方差矩阵的微分矩阵     
       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;


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