智能车制作

 找回密码
 注册

扫一扫,访问微社区

查看: 2225|回复: 1
打印 上一主题 下一主题

求解释卡尔曼滤波算法中几个参数的作用

[复制链接]

2

主题

5

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
419
威望
214
贡献
133
兑换币
136
注册时间
2015-11-7
在线时间
36 小时
毕业学校
海高
跳转到指定楼层
1#
发表于 2017-2-15 13:38:20 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
static const float Q_angle=0.005,Q_gyro=0.001,R_angle=0.5,dt=0.005;
static const char C_0 = 1;
static float q_bias=0,angle_err=0,PCt_0=0,PCt_1=0,E=0,K_0=0,K_1=0,t_0=0,t_1=0;
static float P[2][2] = {
    { 1, 0 },
    { 0, 1 }
};

static float Pdot[4] ={0,0,0,0};

void Kalman_Filter(float angle_m,float gyro_m)          //gyro_m:gyro_measure
{
    attitudeTemp.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 - attitudeTemp.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;
    P[1][0] -= K_1 * t_0;
    P[1][1] -= K_1 * t_1;


    attitudeTemp.angle   += K_0 * angle_err;
    q_bias  += K_1 * angle_err;
    attitudeTemp.angle_dot = gyro_m-q_bias;   
}


回复

使用道具 举报

2

主题

5

帖子

0

精华

中级会员

Rank: 3Rank: 3

积分
419
威望
214
贡献
133
兑换币
136
注册时间
2015-11-7
在线时间
36 小时
毕业学校
海高
推荐
 楼主| 发表于 2017-2-15 13:39:34 | 只看该作者
Q_angle=0.005,Q_gyro=0.001,R_angle=0.5,dt=0.005;这些分别有什么作用?
回复 支持 1 反对 0

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

关于我们|联系我们|小黑屋|亿加合和智能车制作 ( 黑ICP备2022002344号

GMT+8, 2024-5-5 00:54 , Processed in 0.053357 second(s), 30 queries , Gzip On.

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.

快速回复 返回顶部 返回列表