求赐教，卡尔曼滤波，有反向过冲！

阿乾

file:///C:\Users\acer PC\AppData\Roaming\Tencent\Users\1622542013\QQ\WinTemp\RichOle\_))~4}APN`GRRU$EKG%354S.pngfile:///C:\Users\acer PC\AppData\Roaming\Tencent\Users\1622542013\QQ\WinTemp\RichOle\_))~4}APN`GRRU$EKG%354S.png

阿乾

float  Q_angle=0.001;//0.0001;  
float  Q_gyro=0.00001;//0.00003;
float  R_angle=0.01;
float  dt=0.001;//0.005;                         
float  C_0 = 1.0;
float  Q_bias=0, Angle_err=0;
float  PCt_0=0.0, PCt_1=0, E=0.0;
float  K_0=0.0, K_1=0.0, t_0=0.0, t_1=0.0;
float  Pdot[4] ={0,0,0,0};
float  PP[2][2] = { { 1.0, 0 },{ 0, 1.0 } };

void Kalman_Filter(int Gyro,int Accel)       
{
        Angle+=(Gyro - 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 =Accel-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;         
        Gyro_x   = Gyro - Q_bias;


}

阿乾

求看看我的程序，还要调哪个参数，才能去掉反向过冲啊

何俊

阿乾 发表于 2015-9-24 10:11
求看看我的程序，还要调哪个参数，才能去掉反向过冲啊

一边过冲，一边跟踪不够。你的dt设置错了。

阿乾

额，我的定时是1ms，处理一次。我设的是0.001.我调整下试试

阿乾

何俊 发表于 2015-9-24 10:17
一边过冲，一边跟踪不够。你的dt设置错了。

我试试调下dt。我的中断定时是1ms一次采集和处理。我设的是0.001

阿乾

何俊 发表于 2015-9-24 10:17
一边过冲，一边跟踪不够。你的dt设置错了。

求qq，求赐教

、痕迹

float  Q_angle=0.01
float  Q_gyro=0.003
float  R_angle=10
float  dt=0.001;//0.005;   

cleyne

请问楼主怎么解决的？