卡尔曼滤波算法平衡小车

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

。

。抱歉 了。

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

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

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