卡尔曼滤波

在路上520

两轮自平衡电动小车卡尔曼滤波器的设计关键是状态向量的选取，两轮自平衡电动
小车的车体倾角与车体倾斜角速度存在着倒数关系，因此可以选取车体的倾角
（Q_angle）做为状态向量的一个元素，车体的角速度（Q_gyro）做为一个元素，另一
个元素选取加速度计来估计陀螺仪的零位偏差（Q_gyro_bias）, dt=0.01 为卡尔曼滤波采
样时间，观测噪声协方差矩阵（R_angle）。
设定初始数据：//Kalman 滤波，8MHz 的处理时间约 1.8ms；第一步：设定频率
static float angle, angle_dot; //外部需要引用的变量 第二步设定参数 static const
float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;//注意：dt 的取值为 kalman 滤波
器采样时间。
static float P[2][2] = { { 1, 0 },{ 0, 1 } };
static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
void Kalman_Filter（float angle_m,float gyro_m） //gyro_m:陀螺仪实测值
{ angle+=（gyro_m-q_bias） × dt; 先验估计倾角 陀螺仪
Pdot[0]=Q_angle - P[0][1] - P[1][0];先验估计误差协方差的微分
Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] × dt;更新后的协方差矩阵
P[0][1] += Pdot[1] × dt;
P[1][0] += Pdot[2] × dt;
P[1][1] += Pdot[3] × dt;这块看不懂
angle_err = angle_m - angle;先验倾角误差 加速度计
PCt_0 = C_0 × P[0][0]; PCt_1 = C_0 × P[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 × P[0][1];
P[0][0] -= K_0 × t_0;后验估计误差协方差，更新后的协方差矩阵
P[0][1] -= K_0 × t_1[1][0] -= K_1 × t_0;P[1][1] -= K_1 × t_1;
angle += K_0 × angle_err;后验估计倾角误差
q_bias += K_1 × angle_err;后验估计角速度误差
angle_dot = gyro_m-q_bias;输出值后验估计微分（角速度）}

yssdsz

Pdot=APA'+Q   A=（0 -1 0 0）展开计算

在路上520

怎么变成微分了，后面还要积分

yssdsz

啊。。。。。我怎么老是犯傻啊。。。。P=APA'+Q

/*
   * 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 */

感觉先微分在积分可能计算起来会简单一些。。。

yssdsz

你加油。。。我需要去养养精神了。。。。。

linping9656

谢谢楼主

wsdyi100

楼主的头像在我看程序的时候比我晃惨啦，你就不能不点头迈……

zanran8

有点看不懂

Idenktsie

楼主   看的不是很懂啊...

FHFF1234

请问楼主 dt 的取值为 kalman 滤波器采样时间 怎么设定啊 还有着几个参数怎么调啊 能不能给点指点啊