卡尔曼程序问题

夏罗

#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分别代表什么，影响什么，该怎么调节

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

shock

同问啊，还有就是用了卡尔曼，是不是就不用角度归一化了呢？

43380249

犹太马夫

:lol

huangjianwu

没文化

断翅at雄鹰

还在研究卡尔曼啊，呵呵，有耐心