卡尔曼滤波算法相关问题

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;

}

dyl0000

顺便一提，这个程序已经经过验证，是可以用的，在找卡尔曼滤波程序的基友们可以直接用

A楼飘房

谢谢分享，学习了。

于山boy

摄像头的飘过

nimanima

先去找到系统的状态空间模型

gy810986741

有关卡尔曼滤波可以去阿莫论坛上面找找，那里讲的比较详细

dyl0000

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

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

dyl0000

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

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

q1552811728

我也在学习卡尔曼，观点可能不对，希望大家可以交流一下