关键词不能为空

当前您在: 主页 > 英语 >

卡尔曼滤波算法平衡小车

作者:高考题库网
来源:https://www.bjmy2z.cn/gaokao
2021-02-01 18:07
tags:

-

2021年2月1日发(作者:recount)


先上程序,这是抄的不知道谁的代码。



。抱歉 了。


。不过这程序好像都写的差不多



void Kalman_Filter(float Gyro,float Accel)


{



Angle+=(Gyro - Q_bias) * dt;
















Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; /



Pdot[1]= - PP[1][1];



Pdot[2]= - PP[1][1];/



Pdot[3]=Q_gyro;





PP[0][0] += Pdot[0] * dt;






PP[0][1] += Pdot[1] * dt;




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



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








Angle_err = Accel - Angle;





PCt_0 = C_0 * PP[0][0];



PCt_1 = C_0 * PP[1][0];





E = R_angle + C_0 * PCt_0;





K_0 = PCt_0 / E;



K_1 = PCt_1 / E;





t_0 = PCt_0;



t_1 = C_0 * PP[0][1];




PP[0][0] -= K_0 * t_0;



PP[0][1] -= K_0 * t_1;



PP[1][0] -= K_1 * t_0;



PP[1][1] -= K_1 * t_1;






Angle


+= K_0 * Angle_err;



Q_bias


+= K_1 * Angle_err;




Gyro_x




= Gyro - Q_bias;



}


首先是卡尔曼滤波的


5

< p>
个方程




X(k|k-1)=A X(k-1|k-1)+B U(k)


………


.. (1)//


先验估计



P(k|k-1)=A P(k-1|k-1) A



+Q


………


(2)//


协方差矩阵的预测



Kg(k)= P(k|k-1) H



/ (H P(k|k-1) H



+ R)


………


(3)//


计算卡尔曼增益



X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1))


………


(4)


通过卡尔曼增益进行修正



P(k|k)=



I-Kg(k) H



P(k|k-1)


………


(5)//


跟新协方差阵



5


个式子比较抽象,现在直接用实例来说



—,< /p>


对于角度来说,


我们认为此时的角度可以近似认为是上一时刻的角 度值加上上一时刻陀


螺仪测得的角加速度值乘以时间,


因为


d


?


?


dt


?


?



角度微分等于 时间的微分乘以角速度。



但是陀螺仪有个静态漂移(而且还是 变化的)


,静态漂移就是静止了没有角速度然后陀螺仪


也会输出 一个值,这个值肯定是没有意义的,计算时要把它减去。



由此我们得到了当前角度的预测值



Angle



Angle=Angle+(Gyro - Q_bias) * dt;




其中等号左边

Angle


为此时的角度,等号右边


Angle

< p>
为上一时刻的角度,


Gyro


为陀螺仪测


的角速度的值,


dt


是两次滤波之间的时间间 隔。



float



dt=0.005;




















这是程序中的定义



同时



Q_bias


也是一个变化的量。



但是就预测来说认为现在的漂移跟上一时刻是相同的即



Q_bias=Q_bias


将两个式子写成矩阵的形式



Angl e


Q


_


bias


?


1


?


dt


Angle


0


1


Q

_


bia


s


?

dt


0


Gyr


o

< br>



得到上式,这个式子对应于卡尔曼滤波的第一个式子



X(k|k-1)=A X(k-1|k-1)+B U(k)


………


.. (1)//


先验估计



X(k|k-1 )



2


维列向量


1


?


dt


Angle



A



2

维方阵



X(k-1|k-1)



2


维列向量


0


1


Q


_


bias


dt


Angle



B



2


维列向量


U(k)



Gyro


0



Q


_


bias



二,这里是卡尔曼滤波的第二个式子




接着是预测方差阵的预测值,


这里首先要给出两个值,


一个是漂移的噪声,


一个是角度值的


噪声,


(所谓噪声就是数据的方差值)



P(k|k-1)=A P(k-1|k-1) A



+Q



Angle


cov(Angle,Angle)


cov(Q_b ias,Angle)


这里的


Q


为向量


Q


_


bias



的协方差矩阵,即


cov(Angle,Q_bias)


cov(Q_bias)















相< /p>








cov(Angle,Q_bias)< /p>


=0



cov(Q_bias,Angl e)


=0


又由性质可知


cov



x



x

< p>


=D



x


)即方差,所以得到的矩阵如下


-


-


-


-


-


-


-


-



本文更新与2021-02-01 18:07,由作者提供,不代表本网站立场,转载请注明出处:https://www.bjmy2z.cn/gaokao/594322.html

卡尔曼滤波算法平衡小车的相关文章

  • 余华爱情经典语录,余华爱情句子

    余华的经典语录——余华《第七天》40、我不怕死,一点都不怕,只怕再也不能看见你——余华《第七天》4可是我再也没遇到一个像福贵这样令我难忘的人了,对自己的经历如此清楚,

    语文
  • 心情低落的图片压抑,心情低落的图片发朋友圈

    心情压抑的图片(心太累没人理解的说说带图片)1、有时候很想找个人倾诉一下,却又不知从何说起,最终是什么也不说,只想快点睡过去,告诉自己,明天就好了。有时候,突然会觉得

    语文
  • 经典古训100句图片大全,古训名言警句

    古代经典励志名言100句译:好的药物味苦但对治病有利;忠言劝诫的话听起来不顺耳却对人的行为有利。3良言一句三冬暖,恶语伤人六月寒。喷泉的高度不会超过它的源头;一个人的事

    语文
  • 关于青春奋斗的名人名言鲁迅,关于青年奋斗的名言鲁迅

    鲁迅名言名句大全励志1、世上本没有路,走的人多了自然便成了路。下面是我整理的鲁迅先生的名言名句大全,希望对你有所帮助!当生存时,还是将遭践踏,将遭删刈,直至于死亡而

    语文
  • 三国群英单机版手游礼包码,三国群英手机单机版攻略

    三国群英传7五神兽洞有什么用那是多一个武将技能。青龙飞升召唤出东方的守护兽,神兽之一的青龙。玄武怒流召唤出北方的守护兽,神兽之一的玄武。白虎傲啸召唤出西方的守护兽,

    语文
  • 不收费的情感挽回专家电话,情感挽回免费咨询

    免费的情感挽回机构(揭秘情感挽回机构骗局)1、牛牛(化名)向上海市公安局金山分局报案,称自己为了挽回与女友的感情,被一家名为“实花教育咨询”的情感咨询机构诈骗4万余元。

    语文