论互补滤波和卡尔曼滤波

孤行5201314

这是我用的卡尔曼滤波程序

361369499

liheng229 发表于 2016-4-28 17:05
请问 你直接补偿是怎么补偿的啊

比如说你静止的时候，测到加速度计为20度，陀螺仪互补滤波后为22度

假设你车子的平衡位置是0度

这个时候，如果直接跑的话，虽然你设定是0度，但因为温漂，车子是控制到-2度的

也就是说，这个时候车子是比我们预计的要往前倾一些 小了还好，如果前倾大了，会全程加速

直接补偿的话，设定此时的平衡位置为2度，减去温漂得误差后，车子就控制到了0度

九月阁

其实卡尔曼有时候速度环跟不上是你调的太滞后了，至于互补滤波他分一阶和二阶，一阶虽然他的响应速度很好，但是他的稳定性很差也就是毛刺很多，二阶互补滤波如果调的好，可以媲美卡尔曼，目前还是喜欢卡尔曼，比较稳定，至于你说的温漂，你弄个开机自动采集，一般都能处理得了

liheng229

九月阁 发表于 2016-4-30 16:54
其实卡尔曼有时候速度环跟不上是你调的太滞后了，至于互补滤波他分一阶和二阶，一阶虽然他的响应速度很好， ...

谢谢你的详细介绍

liheng229

361369499 发表于 2016-4-29 13:37
比如说你静止的时候，测到加速度计为20度，陀螺仪互补滤波后为22度

假设你车子的平衡位置是0度

可是我们车跑的过程，陀螺仪都会产生很大的偏差，换个型号试试，谢哦

liheng229

孤行5201314 发表于 2016-4-28 23:41
这是我用的卡尔曼滤波程序

嗯，一样的

liheng229

孤行5201314 发表于 2016-4-28 23:41
这是我用的卡尔曼滤波程序

static float P[2][2] = {
                          { 2, 0 },
                          { 0, 2 }
};
float Pdot[4] ={0,0,0,0};
char C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
/**************************************************************************
*Kalman滤波                              
angle_m:   角度
gyro_m:    角速度
**************************************************************************/

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];    //先验估计误差协方差的微分
    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;
   
   
   
   
    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;               //卡尔曼增益
    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_err = angle_m - angle;
    angle   += K_0 * angle_err;     //最优估计值
   
    q_bias  += K_1 * angle_err;
    angle_dot = gyro_m-q_bias;      //输出值（后验估计的微分）=角速度
  
}