智能车制作

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

作者: 在路上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/) Powered by Discuz! X3.2