卡尔曼滤波

仰望星空1

void Kalman_Filter(float angle_m,float gyro_m)
{
        angle+=(gyro_m-q_bias) * dt;
       
        Pdot[0]=Q_angle - p[0][1] - p[1][0];
        Pdot[1]=- p[1][1];
        Pdot[2]=- p[1][1];
        Pdot[3]=Q_gyro;
       
        p[0][0] += Pdot[0] * dt;
        p[0][1] += Pdot[1] * dt;
  p[1][0] += Pdot[2] * dt;
  p[1][1] += Pdot[3] * dt;
       
       
        angle_err = angle_m - angle;
       
       
        PCt_0 = C_0 * p[0][0];
        PCt_1 = C_0 * p[1][0];
       
        En = R_angle + C_0 * PCt_0;
       
        K_0 = PCt_0 / En;
        K_1 = PCt_1 / En;
       
        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;
}


void AD_calculate(void)
{
         while(!ATD0STAT0_SCF);
        acceler=ReadATD(2)+28;                           //角度校正
        gyro=ReadATD(0);
       
       
        acceler*=0.004069;                         //系数换算：2.5/(1.2*512);   
        acceler=asin(acceler);
        gyro*=0.00341;                                   //角速度系数：(3.14/180)* 100/512=0.01364；       
       
Kalman_Filter(acceler,gyro);
}

这个滤波函数的参数怎么调呀，求指教，现在我调的参数是static const float Q_angle=0.0017, Q_gyro=0.00095, R_angle=0.001, dt=0.005;
但是静止时角度值一直浮动，停不下来，晃动的时候有过冲

仰望星空1

这是效果图

萌萌侠

同求呀~~

long1429

逍遥镜风

楼主能不能说下你的系数转换根据设么来的  公式含义是什么  谢谢了

仰望星空1

发表于 昨天 14:33 |只看该作者  楼主能不能说下你的系数转换根据设么来的  公式含义是什么  谢谢了

惭愧呀，我是在网上找的，具体我也不知道呢

linping9656

卡尔曼好吗

Achao

网络上流行的这个卡尔曼应该不是很正确。卡尔曼滤波似乎不是很适合我们直立这种控制模型。只是起到了一个低通的作用。最主要的系数K_0最后会是一个定值常数，，所以这个模型认真分析，其实跟低通算法是一样的，波形越平滑，越滞后。。

仰望星空1

发表于 昨天 14:12 |只看该作者 网络上流行的这个卡尔曼应该不是很正确。卡尔曼滤波似乎不是很适合我们直立这种控制模型。只是起到了一个低通的作用。最主要的系数K_0最后会是一个定值常数，，所以这个模型认真分析，其实跟低通算法是一样的，波形越平滑，越滞后。。

那有正确的没

在路上曹阳

Achao 发表于 2012-4-20 14:12
网络上流行的这个卡尔曼应该不是很正确。卡尔曼滤波似乎不是很适合我们直立这种控制模型。只是起到了一个低 ...

是前面那段求协方差有错误吗，能先微分再积分吗？