智能车制作

标题: 卡尔曼滤波 [打印本页]

作者: 在路上520 时间: 2012-5-27 22:18
标题: 卡尔曼滤波
两轮自平衡电动小车卡尔曼滤波器的设计关键是状态向量的选取，两轮自平衡电动
小车的车体倾角与车体倾斜角速度存在着倒数关系，因此可以选取车体的倾角
（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 时间: 2012-5-27 22:28
Pdot=APA'+Q A=（0 -1 0 0）展开计算

作者: 在路上520 时间: 2012-5-27 22:42
怎么变成微分了，后面还要积分

作者: yssdsz 时间: 2012-5-27 22:48
啊。。。。。我怎么老是犯傻啊。。。。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 时间: 2012-5-27 22:50
你加油。。。我需要去养养精神了。。。。。

作者: linping9656 时间: 2012-5-28 10:55
谢谢楼主

作者: wsdyi100 时间: 2012-11-13 21:06
楼主的头像在我看程序的时候比我晃惨啦，你就不能不点头迈……

作者: zanran8 时间: 2013-3-10 10:50
有点看不懂

作者: Idenktsie 时间: 2013-4-12 11:59
楼主看的不是很懂啊...

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

作者: endlesshai 时间: 2013-4-24 22:34

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

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

作者: FHFF1234 时间: 2013-4-25 12:44

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

没调好

欢迎光临智能车制作 (http://dns.znczz.com/)