卡尔曼陀螺仪、倾角传感器融合 C代码一段语句看不懂

feng741 · 发表于 2010-10-24 12:04:41

void stateUpdate(const float q_m){

float q;
float Pdot[4];

/* 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[0] = Q_angle - P[0][1] - P[1][0]; /* 0,0 */
Pdot[1] = - P[1][1];          /* 0,1 */
Pdot[2] = - P[1][1];          /* 1,0 */
Pdot[3] = 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[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
}
上面一串问号开始到最后，不太懂。

卡尔曼公式 (原文件名:QQ截图未命名4.png)
不是应该向上边1.10式那样么，那样直接就能算出协方差啊，怎么程序中还要先算出Pdot = A*P + P*A' + Q ，再P[0][0] += Pdot[0] * dt;来更新协方差？angle += q * dt;中的dt是积分求角度，可P[0][0] += Pdot[0] * dt;不知道怎么理解了。P.S.从“(equation 22-1)” 看出这个程序应该出自哪篇文章，找了好久没找到，谁知道发一下。谢啦！

源代码ourdev_592360QL9KPM.txt(文件大小:7K) (原文件名:kalman.c.txt)

FpvCamera · 发表于 2010-10-24 12:36:09

最近也是在搞卡尔曼滤波，你这是哪个版本的程序呢，怎么我看的的代码（V0.71h）中没有这么一段？

feng741 · 发表于 2010-10-24 13:12:50

你可能看的是纯卡尔曼滤波吧，这个程序是用陀螺仪、倾角计（或加速度计）实例化的。坐等大侠指点。

feng741 · 发表于 2010-10-24 14:32:37

自己顶一个。

lijieamd · 发表于 2010-10-24 17:56:36

你的资料截图是标准卡尔曼滤波,也就是需要状态过程是线性的,而在非线性系统中,误差如果由线性卡尔曼方程来传播显然是不行的
你所发的程序是将协方差作局部线性化,因此可以继续使用标准卡尔曼方程]
从注释 " Compute the derivative of the covariance matrix " 也可以明显看出来是在对协方差求导, 进行线性近似

FpvCamera · 发表于 2010-10-24 19:38:06

也在看两个函数，有和楼主相同的问题。现在在找其它资料验证。

还有一个问题：如果用这样的方式来做卡尔曼滤波，kalmanUpdate函数在什么时候调用？从注释上看得不太明白

feng741 · 发表于 2010-10-24 22:20:48

derivative这个词理解错误，原来是求导啊，还以为是派生呢。
“对协方差求导, 进行线性近似 ”不太懂啊，lijieamd有相关的资料么？

gasont · 发表于 2010-10-25 09:08:43

建议看秦永元的kalman滤波那本书, 书上有个kalman推倒的过程,有详细的解释

gongyi · 发表于 2010-12-28 10:33:55

回复【楼主位】feng741
-----------------------------------------------------------------------

我对这方面也刚开始研究，暂时还没有能力帮你解答。楼主能否告知一下你的程序是在哪找的？

slim443 · 发表于 2011-3-18 14:50:57

mark

iguess · 发表于 2011-3-18 18:45:26

Pdot = A*P + P*A' + Q /这个适用于连续系统离散化的情况下，试用于程序所处环境

iguess · 发表于 2011-3-18 18:47:41

Pdot = A*P + P*A' + Q /这个适用于连续系统离散化的情况下，试用于程序所处环境，本身是真实世界通过AD采样离散化出来
Pdot=A*P*A'+Q 这个适用于离散系统情况，就是本身系统是离散的，不是AD采样离散化的系统

chenhuanbang · 发表于 2011-3-23 09:57:38

mark

Anteater · 发表于 2011-3-24 12:29:41

回复【楼主位】feng741 梦不落
-----------------------------------------------------------------------

这个是UNC的童鞋翻译的那个kalman介绍吧。。。我前段时间刚刚看过，还照着后面的那个简单例子写了段代码，用到电源控制的代码里去了。。。

直接套用那个介绍解姿态好像不太行，因为解姿态角是一个非线性系统，得用extended kalman filter。就是把非线性系统做局部线性展开，然后再扔到kalman里面去，基本上都是一回事....

shaolinzuqiu · 发表于 2011-4-18 11:13:27

一个简单的卡尔曼滤波的应用，主要用于陀螺仪和加速度计的组合计算真实角度

haoguoying · 发表于 2011-4-18 17:44:03

谁能发我一份卡尔曼滤波计算方法源程序87867861@qq.com在这里先谢谢大家了

thisjoy · 发表于 2011-7-12 21:58:37

mark

nirvanasyl · 发表于 2011-9-25 17:54:56

mark

tang-guo · 发表于 2011-10-5 19:58:24

学习中

thisjoy · 发表于 2011-11-20 13:56:18

mark

zerok · 发表于 2011-11-20 14:08:18

回复【楼主位】feng741 梦不落
-----------------------------------------------------------------------

楼主你好，请问能将程序的头文件：“eyebot.h”，发给我一下吗？lyazk@126.com ，谢谢！

wscjun · 发表于 2011-11-23 17:48:27

学习中

200900207059 · 发表于 2011-11-27 10:56:50

在学卡尔曼滤波

ksh84222 · 发表于 2012-1-5 10:02:55

学习中!

xhzz · 发表于 2012-1-5 10:11:06

mark 卡尔曼

gongyuan1 · 发表于 2012-1-8 13:18:25

回复【10楼】iguess
pdot = a*p + p*a' + q /这个适用于连续系统离散化的情况下，试用于程序所处环境

-----------------------------------------------------------------------

Pdot = A*P + P*A' + Q----中的Q是什么矩阵？是{ {Q_angle, 0},{0,Q_gyro}}吗？转化后应该是：

Pdot[0] = Q_angle; /* 0,0 */
Pdot[1] = - 1; /* 0,1 */
Pdot[2] = - 1; /* 1,0 */
Pdot[3] = Q_gyro; /* 1,1 */

?????

ligongxiaobie · 发表于 2012-2-1 15:54:38

mark

huang91 · 发表于 2012-2-2 16:15:25

同样学习中！一起加油吧。

redwolf310 · 发表于 2012-2-3 23:00:14

mark

dingliming · 发表于 2012-2-5 06:15:47

牛叉

l254068183 · 发表于 2012-2-25 20:10:12

我记得这个有个引文版注释的程序，忘了是哪里找到的了。

linhonggui2007 · 发表于 2012-9-27 01:15:12

楼主，你好，请问下如果不用矩阵的话是不是下面这个公式

传感器的测量值加速度Acc_Mess 陀螺仪 Gyro_Mess
卡尔曼滤波器增益 Kg
卡尔曼滤波后的真实角度值 Acc_Real
系统时钟 Time_Loop

测量不确定度　Q=10

最优角度值的偏差　L

高斯白噪声　T
中间变量 Acc_Media

Acc_Media=Acc_Real+Gyro_Mess*Time_Loop ;//计算瞬时角度值
T＝sqrt(Q＊Q＋L＊L);                               //计算高斯白噪声
Kg=T*sqrt(1/(T*T+Q*Q)) ;                         //计算卡尔曼滤波器的增益
Acc_Real=Acc_Media+Kg*(Acc_Mess-Acc_Media); //计算真实角度值
L=sqrt(1-Kg)*T;                                        //计算真实角度值的偏差

return Acc_Real ;
http://www.amobbs.com/thread-5499119-1-1.html

imwafush · 发表于 2012-11-29 16:53:35

mark,学习中！

墨雪 · 发表于 2012-12-12 21:12:17

Anteater 发表于 2011-3-24 12:29
回复【楼主位】feng741 梦不落
---------------------------------------------------------------------- ...

可以加你为好友吗，863046425详谈卡尔曼

hnstray · 发表于 2013-3-1 14:22:52

有没有解决啊，楼主

chushibinsaobao · 发表于 2013-12-29 11:08:21

怎么没有大师来呢！

zhaoylx@163.com · 发表于 2014-1-13 13:04:45

这个算法中，Pdot是P阵的微分，照着卡尔曼滤波的五个式子中的P阵的算法，把dt提出来，舍掉高次项，就得到了P的微分（矩阵对时间的微分）
      Pdot[0] = Q_angle - P[0][1] - P[1][0];       /* 0,0 */
      Pdot[1] = - P[1][1];                      /* 0,1 */
      Pdot[2] = - P[1][1];                      /* 1,0 */
      Pdot[3] = Q_gyro;                      /* 1,1 */

yhybear0 · 发表于 2014-1-17 12:49:01

看不懂，帮顶！

watermelon_ok · 发表于 2014-1-23 20:09:41

实际做起来还是很难滴

Nekos · 发表于 2014-7-6 13:06:01

楼主能说明下，程序里的 pdot ， p 数组还有PCt_0, PCt_1, K_0, K_1, t_0, t_1 这些变量在卡尔曼的公式里各代表什么下。

yelvAVR · 发表于 2014-7-31 11:34:41

linhonggui2007 发表于 2012-9-27 01:15
楼主，你好，请问下如果不用矩阵的话是不是下面这个公式

传感器的测量值加速度Acc_Mess 陀螺仪 Gyro_M ...

你好，
非常感谢，你的公式是直接按照卡尔曼关于温度测量直接推导出的。
看完很有触发，很有帮助。
为了防止误导，建议提：
加速度作用：
加速度测量的是加速度值，再结合重力加速度值反推出来角度值。

卡尔曼 陀螺仪、倾角传感器融合 C代码 一段语句看不懂

阿莫论坛20周年了！感谢大家的支持与爱护！！

卡尔曼陀螺仪、倾角传感器融合 C代码一段语句看不懂