卡尔曼 陀螺仪、倾角传感器融合 C代码 一段语句看不懂
本帖最后由 Ricky 于 2014-5-8 14:10 编辑void stateUpdate(const float q_m){
float q;
float Pdot;
/* Unbias our gyro */
q = q_m - q_bias;
/*
* Compute the derivative of the covariance matrix
* (equation 22-1)
* Pdot = A*P + P*A' + Q //？？？？？？？？？？？？
*
*/
Pdot = Q_angle - P - P; /* 0,0 */
Pdot = - P; /* 0,1 */
Pdot = - P; /* 1,0 */
Pdot = Q_gyro; /* 1,1 */
/* Store our unbias gyro estimate */
rate = q;
/*
* Update our angle estimate
* angle += angle_dot * dt
* += (gyro - gyro_bias) * dt
* += q * dt
*/
angle += q * dt;
/* Update the covariance matrix */
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
}
上面一串问号开始到最后，不太懂。
卡尔曼公式 (原文件名:QQ截图未命名4.png)
不是应该向上边1.10式那样么，那样直接就能算出协方差啊，怎么程序中还要先算出Pdot = A*P + P*A' + Q ，再P += Pdot * dt;来更新协方差？angle += q * dt;中的dt是积分求角度，可P += Pdot * dt;不知道怎么理解了。P.S.从“(equation 22-1)” 看出这个程序应该出自哪篇文章，找了好久没找到，谁知道发一下。谢啦！
我说，用卡尔曼滤波，看懂太累了。不如知道导入哪几个量之后输出啥近似实际值=。= 呵呵，对我帮助很大
:lol:lol:lol:lol:lol:lol:lol
:D:D:D:D:D:D:D:D:D
;P;P;P;P;P;P;P;P;P
页:
[1]