卡尔曼滤波

在路上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;输出值后验估计微分（角速度）}

FHFF1234

endlesshai 发表于 2013-4-24 22:34
同求啊，你调好了吗，怎么调的呢

没调好

endlesshai

FHFF1234 发表于 2013-4-17 08:51
请问楼主 dt 的取值为 kalman 滤波器采样时间 怎么设定啊 还有着几个参数怎么调啊 能不能给点指点啊

同求啊，你调好了吗，怎么调的呢

FHFF1234

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

Idenktsie

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

zanran8

有点看不懂

wsdyi100

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

linping9656

谢谢楼主

yssdsz

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

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

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