卡尔曼滤波后的图 请老师指点

silence1221

  卡尔曼参数Q_angle=0.001, Q_gyro=0.003, R_angle=0.067, dt=0.005;   
  现在车子能直立，但是很不稳定  先是直立，然后来回抖动，越摆越大，然后倒下。。。。现在怎么调这个波形呢

滑民航

silence1221 发表于 2012-5-13 17:55
//是根据陀螺仪计算的角速度

你们的陀螺仪过冲大吗   

silence1221

波形是在桌面上用手来回摆动车子测得的

吉他手

（1）首先你需要清楚，滤波后的角度信号需要逼近加速度计中没有带有运动噪声的角度；从你的波形显示的滤波后的角度与加速度信号大体上呈现反相特点；逻辑上应该是同相；这一点通过调整后面的角度控制的PI参数中的极性可以修改；
（2）之所以出现震荡，另外一个问题就是控制的“极性”。角度的P，I都应该是负反馈，如果某一项极性反了，就会造成控制输出发散，车模震荡倒下。请检查一下。
（3）如果你对于卡尔曼滤波不是太清楚，可以先实现参考方案中的方法，它简单，物理概念清楚。获得调试的经验之后再研究卡尔曼滤波。以免在此耽搁太多的时间。毕竟后面还有更多的参数需要调整呢。
  从本质上讲，论坛上所给出的卡尔曼滤波的算法和参考方案方法所得到的结果是一样的。

silence1221

吉他手 发表于 2012-5-12 15:33
（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;
        //更新协方差矩阵  其中
*
* state_update is called every dt with a biased gyro measurement
* by the user of the module.  It updates the current angle and
* rate estimate.
*
* The pitch gyro measurement should be scaled into real units, but
* does not need any bias removal.  The filter will track the bias.
*
*          A = [ 0 -1 ]
*              [ 0  0 ]
*/
*
         * Compute the derivative of the covariance matrix
         * (equation 22-1)
         *        Pdot = A*P + P*A' + Q
         *
         */
        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; //R_angle为测量噪声（white gaussian noise）

       //更新卡尔曼滤波增益
        K_0 = PCt_0 / E;//Kk
        K_1 = PCt_1 / E;
        //t_0,t_1为测量系统参数，为下面的后验协方差提供。

        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;//输出值（后验估计）的微分 = 角速度
}

silence1221

滑民航 发表于 2012-5-13 10:24
想问下  你的卡尔曼是不是用这个
void Kalman_Filter(float angle_m,float gyro_m)                       ...

恩 是的  怎么了

silence1221

吉他手 发表于 2012-5-12 15:33
（1）首先你需要清楚，滤波后的角度信号需要逼近加速度计中没有带有运动噪声的角度；从你的波形显示的滤波后 ...

吉他手老师，昨天我又重新调了下滤波，这是现在的波形图了，你看看哪里还不对。



直接用手摆动小车测试的波形 红色是加速度计 黄色是滤波之后

滑民航

silence1221 发表于 2012-5-13 11:29
恩 是的  怎么了

那个gyr-m是不是陀螺仪的测量值吗     还是陀螺仪积分后的角度值

silence1221

滑民航 发表于 2012-5-13 16:29
那个gyr-m是不是陀螺仪的测量值吗     还是陀螺仪积分后的角度值

   //是根据陀螺仪计算的角速度