平衡小车倾角中的卡尔曼滤波

　　今年过年期间无聊，网购了个二轮平衡小车玩玩，想着填补下非科班出身的嵌软没进过实验室玩平衡小车的遗憾。在看卖家提供的demo时发现，有个卡尔曼滤波是我没接触过的，就去看提供的视频学习，结果明显又是网上抄个代码能用就以为自己懂了的那种，全程不知道在噏乜柒。

　　上网搜了下，代码好像都同样的代码，有挺多博客分析了这套代码，花了半天时间去学习，呵呵数学相关的代码，想每一行的依据都弄清楚，还是自己推吧。

 1 float K1=0.02; 
 2 float angle, angle_dot;     
 3 float Q_angle=0.001; 
 4 float Q_gyro=0.003;
 5 float R_angle=0.5;
 6 float dt=0.005;            
 7 char  C_0 = 1;
 8 float Q_bias, Angle_err;
 9 float PCt_0, PCt_1, E;
10 float K_0, K_1, t_0, t_1;
11 float Pdot[4] ={0,0,0,0};
12 float PP[2][2] = { { 1, 0 },{ 0, 1 } };
13 
14 void Filter_Kalman(float Accel,float Gyro)      
15 {
16     angle+=(Gyro - Q_bias) * dt;
17     Pdot[0]=Q_angle - PP[0][1] - PP[1][0];
18     Pdot[1]=-PP[1][1];
19     Pdot[2]=-PP[1][1];
20     Pdot[3]=Q_gyro;
21     PP[0][0] += Pdot[0] * dt;
22     PP[0][1] += Pdot[1] * dt;
23     PP[1][0] += Pdot[2] * dt;
24     PP[1][1] += Pdot[3] * dt;
25 
26     Angle_err = Accel - angle;
27 
28     PCt_0 = C_0 * PP[0][0];
29     PCt_1 = C_0 * PP[1][0];
30 
31     E = R_angle + C_0 * PCt_0;
32 
33     K_0 = PCt_0 / E;
34     K_1 = PCt_1 / E;
35 
36     t_0 = PCt_0;
37     t_1 = C_0 * PP[0][1];
38 
39     PP[0][0] -= K_0 * t_0;
40     PP[0][1] -= K_0 * t_1;
41     PP[1][0] -= K_1 * t_0;
42     PP[1][1] -= K_1 * t_1;
43 
44     angle   += K_0 * Angle_err;
45     Q_bias  += K_1 * Angle_err;
46     angle_dot   = Gyro - Q_bias;
47 }

 

 

　　代码和草稿见上面了，代码除了函数名，其他内容感觉全网都一样的了；手写的推导过程，可能有typo，但没必要纠结。推导过程是能完全指导代码了的。而细节方面有两个不一致：1)第6行dt是个HyperParameter，这玩意没跟Q_angle,Q_gyro挂钩，但是代码里第21，24行却都*了dt，这在改dt时会导致Q_angle,Q_gyro与预设出现偏差；2)第7行C_0我推导过程中是直接认为const的1，代码里把他当成了HyperParameter可以用来调。

 

posted @ 2025-05-01 12:08  蓝bleu  阅读(53)  评论(0)    收藏  举报