扔一个NXC写的卡尔曼滤波代码（函数形式）

blackblue

好长时间不发贴了，交点作业，发个前期写的并能在NXC上应用的（已经经过本人验证能用的）KALMAN FILTER:
注： Q_angle 0.001  Q_omega 0.003 R_angle 0.03 这三个参数怎么取值，我也正在学习中，所以别问我为什么这么取？

//简化版kalman filter  for NXC
//输入参数：Angle_acc_x 加速计计算出的某一轴与水平自然座标系之夹角；
//                      gyro_Raw  与上一参数对应的某轴的陀螺输出的角速度值；
//                      tInterval ：积分时间
//-----------------------------------------------------------------------------------
//返回值：  kal_angle:滤波后的角度值
//------------------------------------------------------------------------------------


#define Q_angle 0.001
#define Q_omega 0.003
#define R_angle 0.03
float Klm_angle;
float bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;


float   Kalman_Filter(float Angle_acc_x,float gyro_Raw,float tInterval )
  {
     Klm_angle += ( gyro_Raw - bias) * tInterval;
     P_00 += -(P_10 + P_01) * tInterval + Q_angle *tInterval;
     P_01 += -P_11 * tInterval;
     P_10 += -P_11 * tInterval;
     P_11 += +Q_omega * tInterval;

     float K_0 = P_00 / (P_00 + R_angle);
     float K_1 = P_10 / (P_00 + R_angle);

     bias += K_1 * ( Angle_acc_x - Klm_angle);
     Klm_angle += K_0 * (Angle_acc_x - Klm_angle);
     P_00 -= K_0 * P_00;
     P_01 -= K_0 * P_01;
     P_10 -= K_1 * P_00;
     P_11 -= K_1 * P_01;
     return klm_angle;
}
     //kalman filter end

WALL·E

有点深奥，没看懂求解释

sirius

请教一下：angle_acc_x和gyro_Raw都是传感器返回的值吗？看来如果没有加速度传感器，就没法采用这种滤波方式了？

gwang_10

楼主大大，我想参考一下你的自平衡小车的程序可以吗？我琢磨了好久还是不行，麻烦了~~342732794@qq.com

blackblue

#define PORT S1
#define Adresse (0x68<<1)  
#define OFFSET_SAMPLES 200

byte init1[] = {Adresse, 0x6B, 0x80};
byte init2[] = {Adresse, 0x6B, 0x03};
byte init3[] = {Adresse, 0x1A, 0x00};
byte init4[] = {Adresse, 0x1B, 0x18};
byte read_gyro[] = {Adresse, 0x43};

byte nbytes;
byte data[];

int angle_speed_0= 0;
int angle_speed=0;
int angle=0;
float angle_P=1.2;
float angle_D=10;
int last_angle_error=0;

int route=0;
float route_P_factor=0.1;
float route_D_factor=25;
int last_route_error=0;

void GetGyroOffset()
{
  float gSum;
  int  i, gMin, gMax, g;

  ClearScreen();
  TextOut(0, LCD_LINE1, "  6050 Segway   ");

  TextOut(0, LCD_LINE4, "Lay robot down");
  TextOut(0, LCD_LINE5, "flat to get gyro");
  TextOut(0, LCD_LINE6, "offset.");
  
    gSum = 0.0;
    gMin = 1000;
    gMax = -1000;
    for (i=0; i<OFFSET_SAMPLES; i++) {
     
      
      I2CWrite(PORT, 6, read_gyro);
      while(I2CStatus(PORT, nbytes)>0);
          if (nbytes == 6)
          {
           I2CRead(PORT, 6, data);
           g = (((data[0]<<8) | data[1])) ;
          }
              
      if (g > gMax)
        gMax = g;
      if (g < gMin)
        gMin = g;

      gSum += g;
      Wait(5);
    }

  angle_speed_0 = gSum / OFFSET_SAMPLES +0.8;
}


task main()
{
     
      SetSensorLowspeed(PORT);
      Wait(25);

      I2CWrite(PORT, 0, init1);
      while(I2CStatus(PORT, nbytes)>0);

      I2CWrite(PORT, 0, init2);
      while(I2CStatus(PORT, nbytes)>0);

      I2CWrite(PORT, 0, init3);
      while(I2CStatus(PORT, nbytes)>0);

      I2CWrite(PORT, 0, init4);
      while(I2CStatus(PORT, nbytes)>0);

      Wait(50);
      
      GetGyroOffset();
      Wait(50);
      ResetRotationCount(OUT_B);
      while (1)
      {
              I2CWrite(PORT, 6, read_gyro);
              while(I2CStatus(PORT, nbytes)>0);
              if (nbytes == 6)
              {
                  I2CRead(PORT, 6, data);

                         angle_speed = (((data[0]<<8) | data[1])) - angle_speed_0;
                         angle_speed/=50;
               }

            angle  += angle_speed;

            int error_angle =  0 - angle;
            int angle_PTerm = error_angle * angle_P;
            int angle_DTerm = angle_speed * angle_D;

            route = (MotorRotationCount(OUT_B));
            int error_route = 0 - route;
            int route_PTerm = error_route * route_P;
            int route_DTerm  = (error_route - last_route_error) * route_D;
            last_route_error = error_route;

            int PD = angle_PTerm - angle_DTerm - route_PTerm - route_DTerm;

            if (PD >100) {PD = 100;}
            if (PD < -100) {PD = -100;}

            OnFwd(OUT_BC, PD);
        }


}