智能车制作

标题: 卡尔曼程序问题 [打印本页]

作者: 夏罗 时间: 2012-4-15 20:34
标题: 卡尔曼程序问题
#include "main.h"
  extern  float angle, angle_dot; //外部需要引用的变量
  #define M    20
//-------------------------------------------------------
  float Q_angle=0.002, Q_gyro=0.0015, R_angle=0.0001, dt=0.002;
//注意：dt的取值为kalman滤波器采样时间;
  float P[2][2] = {
      { 1, 0 },
      { 0, 1 }
      };
  float Pdot[4] ={0,0,0,0};
  const char C_0 = 1;
  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:gyro_measure
  {
  angle+=(gyro_m-q_bias) * dt;//先验估计
  Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
  Pdot[1]=- P[1][1];
  Pdot[2]=- P[1][1];
  Pdot[3]=Q_gyro;
  P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  angle_err = angle_m - angle;//zk-先验估计
  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;//Kk
  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;

  angle += K_0 * angle_err;//后验估计
  q_bias += K_1 * angle_err;//后验估计
  angle_dot = gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度

OutData[1]  = angle;
QingJiao=angle;
Gyro_Data=  angle_dot;
//OutData[3]  = Gyro_Data;
OutPut_Data();

}

void AD_calculate(void)
{
            float acceler=0,gyro=0;
            int i;
            for( i=0;i<20;i++){

            while(!ATD0STAT0_SCF)             //等待转换完成
               gyro= (float)ATD0DR0;          //读取陀螺仪AD值
               acceler+= (float)ATD0DR2;    //加速度计AD值

      }
      acceler= acceler/M-1400; //加速度计
      gyro=gyro -1940;       // 陀螺仪
   //acceler*=0.004069;    //系数换算：2.5/(1.2*512);
   //acceler=asin(acceler);
   //gyro*=0.00341;       //角速度系数：(3.14/180)* 100/512=0.01364；

      OutData[0] = acceler; //加速度计
      OutData[2]  = gyro;    //陀螺仪

   Kalman_Filter(acceler,gyro);
}
大家觉得我这程序有什么不合理的，  Kalman_Filter(acceler,gyro);调用的两参数是不是这样的，是不是必须转换成对应的角度和角速度才可以啊，还有卡尔曼滤波的参数  Q_angle, Q_gyro, R_angle分别代表什么，影响什么，该怎么调节

希望做过的各位多多指导！

作者: 断翅at雄鹰 时间: 2012-4-15 21:20
还在研究卡尔曼啊，呵呵，有耐心

作者: 没文化 时间: 2012-4-23 12:14

作者: huangjianwu 时间: 2013-4-17 21:38

作者: 犹太马夫 时间: 2015-2-6 20:06

:lol

作者: 43380249 时间: 2015-2-13 14:34

作者: shock 时间: 2015-3-22 15:17
同问啊，还有就是用了卡尔曼，是不是就不用角度归一化了呢？

欢迎光临智能车制作 (http://111.231.132.190/)