搜索
bottom↓
回复: 0

卡尔曼滤波请教

[复制链接]
(140085904)

出0入0汤圆

发表于 2016-6-22 16:16:13 | 显示全部楼层 |阅读模式
下面是卡尔曼滤波程序的一种,在很多程序里都看到过,但是不知道如何使用,请大家讲解一下
static float gyro,acceler,Vref=2.048,fangdabeishu=24.6;
static float acc,gg,acce;

static float angle, angle_dot;                 
static const float Q_angle = 0.001, Q_gyro = 0.003, R_angle = 0.5, dt = 0.006;
static float PP[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;
static float gyro,acceler;

void Kalman_Filter(float angle_m,float gyro_m)               
{
          angle += (gyro_m-q_bias)*dt;

          Pdot[0] = Q_angle-PP[0][1]-PP[1][0];
          Pdot[1] = -PP[1][1];
          Pdot[2] = -PP[1][1];
          Pdot[3] = Q_gyro;

          PP[0][0] += Pdot[0]*dt;
          PP[0][1] += Pdot[1]*dt;
          PP[1][0] += Pdot[2]*dt;
          PP[1][1] += Pdot[3]*dt;

          angle_err = angle_m-angle;

          PCt_0 = C_0*PP[0][0];
          PCt_1 = C_0*PP[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*PP[0][1];

          PP[0][0] -= K_0*t_0;
          PP[0][1] -= K_0*t_1;
          PP[1][0] -= K_1*t_0;
          PP[1][1] -= K_1*t_1;

          angle += K_0*angle_err;
          q_bias += K_1*angle_err;
          angle_dot = (gyro_m-q_bias);
}

论坛公益广告: VIP+与VIP++福利 >>

回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子论坛 ( 公安交互式论坛备案:44190002001997 粤ICP备09047143号-1 )

GMT+8, 2020-11-30 01:01

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

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