请教：姿态结算中，求得四元数后，如何使用卡尔曼滤波？

zz274510110

如题，

使用龙格库塔法更新四元数，接着要使用巴尔曼滤波器将它滤波。

但以前只接触过单变量的卡尔曼，这次是四个变量，

请知道的坛友指教下原理和公式~~~

这是更新四元数的后半部分程序：

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. }

复制代码

zywei_09

不是很清楚，不过单变量要都变成矩阵。

aishadow

完整代码发下。

number007cool

怎么就没有下文了

kexiao

单变量的卡尔曼滤波器

qwe2231695

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

jmp2002911911

qwe2231695 发表于 2013-5-10 02:38
我觉得要是单变量卡尔曼用起来的话，先把加速度做卡尔曼（我现在用的是20次滑动平均），在传入四元素里面融 ...

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

qwe2231695

jmp2002911911 发表于 2013-5-10 10:05
不要把卡尔曼和普通滤波混淆了。你对加速度做的只是个平均滤波，那可算不上卡尔曼。 ...

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

想飞的四轴

楼主是怎么解决问题的  我呀做到四元数了 ，但是不知道下面该怎么滤波了。。

chushibinsaobao

楼主 ，有个小问题想请教一下，程序中的协方差是怎么确定的啊

zlpvch

正在入门卡尔曼

木君之上

期待大神回答，楼主有个问题，对四元数做卡尔曼滤波之后呢，然后还原成陀螺仪的角速度，然后积分或者融合求姿态，然后用PID控制？

davis201005

学习中...

bli19

哦，学习下。

onev

chushibinsaobao 发表于 2013-12-20 18:09
楼主 ，有个小问题想请教一下，程序中的协方差是怎么确定的啊

过去快一年了，不知道你的问题有没有解决？ 最近想做四元数的卡尔曼滤波，但 系统过程的协方差Q  和 测量过程的协方差R 不知道如何确定，求指教！四元数是4X1的矩阵，不知道这协方差怎么弄！！！

ttoto

我认为你要用欧拉角来理解四元数。这样子其实是一个空间的向量+转角两个变量。

lou

onev 发表于 2014-11-7 18:40
过去快一年了，不知道你的问题有没有解决？ 最近想做四元数的卡尔曼滤波，但 系统过程的协方差Q  和 测量 ...

想问一下您的问题解决了吗？ 我现在也在弄四元数的卡尔曼，但不清楚应该怎么确定系统过程的协方差Q 和测量过程的协方差R ，能解答一下吗？ 多谢！