卡尔曼滤波用的是非矩阵还是矩阵的?

subingkiun

在网上找到的非矩阵的卡尔曼滤波的程序,非矩阵的,有人用他直立过么?

volatile float QingJiao = 0;
volatile float Gyro_Data = 0;
void Kalman(void)
{
   float Q =1,R = 300;
   static float RealData = 0,RealData_P = 0;
   float NowData = 0,NowData_P = 0;
   float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0;
   float Acc_x = 0,Acc_z = 0, Gyro = 0;
   static float his_acc = 0.0,his_accx = 0.0,his_accz = 0.0;
   
   while(!ATD0STAT0_SCF);   //等待转换完成
        
    Acc_x = (float)ATD0DR1;
    Acc_z = (float)ATD0DR2;
    Gyro  = (float)ATD0DR0;
   
   if(Gyro > 4090) Gyro += 1000;
   else
   if(Gyro > 4084) Gyro += 500;
   
   if(Gyro < 55)   Gyro -= 1000;
   else
   if(Gyro < 60)   Gyro -= 500;
      
    Acc_x = Acc_x - 2342.0;
    Acc_z = Acc_z - 2076.0;
    Gyro  = Gyro  - 2048.0;
    Gyro_Data = Gyro;
   
    OutData[0] = Gyro_Data;
    accelerometer_angle = atan2f(-Acc_x,Acc_z);
    OutData[1] = accelerometer_angle*1000;

    gyroscope_rate = Gyro*0.0023;         //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.
   
                                          //(3300/4096)/(0.67*9.1)*(3.14/180) =  0.0023
   
    NowData = RealData + gyroscope_rate*0.01;                 
    //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
    NowData_P = sqrt(Q*Q+RealData_P*RealData_P);            
    //2.计算预估计协方差矩阵   P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
    Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R));
    //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
    RealData = NowData + Kg*(accelerometer_angle - NowData);
    //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
    RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);           
    //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)
   
    QingJiao =  RealData;  
   
   
    OutData[2] = QingJiao*1000;
    OutPut_Data();
}
他用的AD转换是12位精度的。但是输出的波形是这样的
那我要调什么参数？
是不是陀螺仪的比例大了？
真心求指教！！！
红色的是陀螺仪
黄色是加速度的角度
蓝色的是融合后的。

subingkiun

国际惯例，沙发
求指教

yukunlinykl

呀！还有矩阵版本。
学到了，学到了。

为了明天_For_To

两个都试过  不行  要么过冲非常大  要么没过冲就是太滞后了，完全不能接受，最后还是换成互补滤波了

hmit

这卡尔曼被简化的有点厉害。A，B，H，I都是1了。
RealData = NowData + Kg*(accelerometer_angle - NowData);
^                              ^                     ^                   ^
|                               |                       |                      \           
最优估计值              Q/（Q+R）     观测值（加速度计 ）     估计值（陀螺仪）     

你的程序 Q =1,R = 300;这样Kg很小几乎不起作用。懂了吗？         

subingkiun

hmit 发表于 2013-1-24 14:41
这卡尔曼被简化的有点厉害。A，B，H，I都是1了。
RealData = NowData + Kg*(accelerometer_angle - NowDat ...

是要调Q跟R的是么？
dt要调么？
今天下午看了一下这个资料。
大概知道怎么个流程。
现在调出了这样的波形，不知道行不行。。
蓝色是滤波后的；
黄色是加速度的输出；
红色的是陀螺仪的输出。
希望指出不足，

、煈の°絮だ

滞后有点严重啊

、煈の°絮だ

subingkiun 发表于 2013-1-24 21:05
是要调Q跟R的是么？
dt要调么？
今天下午看了一下这个资料。

这个真不错啊

hmit

subingkiun 发表于 2013-1-24 21:05
是要调Q跟R的是么？
dt要调么？
今天下午看了一下这个资料。

差不多了。只不过陀螺仪最后的一段波形怎么回事？
dt是你调用卡尔曼滤波的周期。0.005就是5ms调用一次卡尔曼滤波。你10ms一次有点慢。

hmit

还有你的陀螺仪信号与角度信号极性反了。在调PD要加个负号。还是推荐用矩阵的那个。他可以把角速度也很好的滤一下。