如何在得到四元数之后进行卡尔曼滤波？

用卡尔曼滤波处理四元数，以前用过单变量的卡尔曼滤波，但是这一次需要处理四个变量，有些不太清楚了，有做过的坛友指教一下原理和公式~~下面是我四元数的后半部分程序：

1. // integrate quaternion rate and normalise
   
2.   q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
   
3.   q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
   
4.   q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
   
5.   q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
   
6.   
   
7.   // normalise quaternion
   
8.   norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
   
9.   q0 = q0 * norm;
   
10.   q1 = q1 * norm;
    
11.   q2 = q2 * norm;
    
12.   q3 = q3 * norm;

复制代码

这是单变量的卡尔曼滤波器：

1. float KalmanGain;//  卡尔曼增益
   
2. float EstimateCovariance;//估计协方差
   
3. float MeasureCovariance;//测量协方差
   
4. float EstimateValue;//估计值
   
5.   
   
6. void KalmanFilterInit(void)
   
7. {
   
8.         EstimateValue = 0;
   
9.         EstimateCovariance = 1;
   
10.         MeasureCovariance = 2;
    
11. }
    
12. 
    
13. float KalmanFilter(float Measure)
    
14. {
    
15.         //计算卡尔曼增益
    
16.         KalmanGain=EstimateCovariance*sqrt(1/(EstimateCovariance*EstimateCovariance+MeasureCovariance*MeasureCovariance));
    
17. 
    
18.         //计算本次滤波估计值
    
19.         EstimateValue=EstimateValue+KalmanGain * (Measure-EstimateValue);
    
20.         
    
21.         //更新估计协方差
    
22.         EstimateCovariance=sqrt(1-KalmanGain) * EstimateCovariance;
    
23.         
    
24.         //更新测量方差
    
25.         MeasureCovariance=sqrt(1-KalmanGain) * MeasureCovariance;
    
26.         
    
27.         //返回估计值
    
28.         return EstimateValue;
    
29. }

复制代码

首先，你要把单变量要都变成矩阵。

我觉得要是单变量卡尔曼用起来的话，先把加速度做卡尔曼（我现在用的是20次滑动平均），在传入四元素里面融合，应该会有不少的提升。

不要把卡尔曼和普通滤波混淆了。你对加速度做的只是个平均滤波，那可算不上卡尔曼。

是的，我说的是代替。单变量卡尔曼只能滤波不能融合。