矩阵式卡尔曼滤波的问题

bobo_12300

我用的是矩阵式卡尔曼滤波，代码如下

float dt=0.005;   //采样周期
float P[2][2]={{1,0},{0,1}};  
//float  g_fcarspeed;
float  q_bias=0;
float  rate, angle_dot;
float R_angle=0.001; //加速度计测量误差协方差
float Q_angle = 0.001;  //系统过程噪声协方差矩阵
float Q_gyro  = 0.0015; //数值越小表明越信任
//A=[0 -1]
//  [0  0]
float q;
float Pdot[4];
void  state_update(void)
{
  
//float angle_err;
// float PHt_0 , PHt_1;
// float E,K_0,K_1,Y_0,Y_1;
  
  q = g_fanglespeed - q_bias; //先验估计   当前角速度=测量值-偏差
  g_fcarspeed += q*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;
  rate = q;   //保存陀螺仪的无偏估计值
  
  //更新协方差矩阵
  P[0][0]+=Pdot[0]*dt;
  P[0][1]+=Pdot[0]*dt;
  P[1][0]+=Pdot[0]*dt;
  P[1][1]+=Pdot[0]*dt;
  
}

float h_0 = 1;
float angle_err;
float PHt_0 , PHt_1;
float E,K_0,K_1,Y_0,Y_1;
float angle_dot ;

void kalman_update(void)
{
  angle_err =g_fgravityangle - g_fcarspeed;   //计算测量角度和观测的角度的误差
   
  PHt_0=h_0*P[0][0];
  PHt_1=h_0*P[1][0];
  
  //计算误差估计 E=HPH' + R
   E = R_angle+(h_0*PHt_0);
                              
//计算卡尔曼增益
   K_0 = PHt_0 *1.0/E;
   K_1 = PHt_1 *1.0/E;

//更新协方差矩阵
  Y_0 = PHt_0;
  Y_1 = h_0*P[0][1];

  P[0][0] -= K_0*Y_0;
  P[0][1] -= K_0*Y_1;
  P[1][0] -= K_1*Y_0;
  P[1][1] -= K_1*Y_1;

//更新状态估计  Xnew = X + K*error;
//err是加速度计测得角度和估计的角度的差

g_fcarspeed  += K_0*angle_err;
q_bias += K_1*angle_err;
angle_dot=g_fanglespeed  - q_bias ;
}

但是输出的角度一直不变是怎么回事？
还奇怪啊

除却巫山不是云

之前看过别人对其的一些看法：卡尔曼滤波 实现性最优解的一种求解算法，他必须有根据建模的预测值和传感器数据采集的测量值 才能够进行相应的滤波算法。
卡尔曼滤波要点：建立系统模型，确定滤波量 确定预测量 确定测量量 进行五个方程计算滤波
在陀螺仪和加速度传感器滤波的融合中，我们得首先建立起模型（融合只适用于重力情况下，若加速度传感器因为有其他力引入会造成队伍，故只用来进行水平姿态的测量）
这里我重点说一下模型的特点，首先模型是一个2维模型，故在写程序的时候需要先把卡尔曼滤波的五个方程用线性代数的方法解算出来，这样才能够进行相应的卡尔曼滤波程序的编写。（一定注意这是2维的卡尔曼滤波，根据论文中的五个方程进行相应的线性代数化简才能够看懂程序），一定注意程序是在用线性代数解算后的方程，如果不明白可以自己推到以下， 这里的我们的预测量就是用陀螺仪测量的数据，而测量量就是加速度传感器测量的数据。
给你这个看看，线性代数学得好的话应该能推出来，之我试推过有些不一样，线性代数没学好
推出来分享一下。。嗷嗷

二百五

除却巫山不是云 发表于 2013-5-29 18:21
之前看过别人对其的一些看法：卡尔曼滤波 实现性最优解的一种求解算法，他必须有根据建模的预测值和传感器数 ...

kalman我推过，就一点不同，只是因为太小所以省略了。加你QQ一起讨论一下。怎么样。

除却巫山不是云

二百五 发表于 2013-5-31 23:06
kalman我推过，就一点不同，只是因为太小所以省略了。加你QQ一起讨论一下。怎么样。

寒假拿着书对着PDF推不出，现在都忘光了，你推出来了么，分享一下过程QQ:894985240