求Kalman滤波中状态变量选择四元数还是欧拉角问题的解答

leicheng · 发表于 2014-2-25 21:02:31

本帖最后由 leicheng 于 2014-2-25 21:10 编辑

看了一些有代表性的卡尔曼滤波帖子，知道状态变量一般会选择四元数，如果在卡尔曼滤波中应用四元数，可以对加计和磁强计通过高斯-牛顿法解出四元数的测量值，对四元数微分方程通过龙格库塔法解出四元数的更新值，进行卡尔曼滤波。
我的问题是，如果状态变量不是选择四元数而是选择欧拉角，状态方程中通过陀螺仪进行欧拉角的更新，对测量量，通过加计和磁强利用公式计算出某个时刻的欧拉角，再进行卡尔曼滤波。这种思路有问题吗？
我的感觉是直接用欧拉角计算量小些，虽然有奇异点等问题。四元数涉及到一些数值计算，感觉还不如直接用欧拉角。请教各位达人高手解答~

情迷MJ比莉珍 · 发表于 2014-2-26 10:38:24

楼主的问题好深奥啊！！

leicheng · 发表于 2014-2-26 22:06:16

还有个问题，计算测量值的四元数，能否不采用高斯牛顿法，而是先通过计算加计和磁强计得到姿态角，再用下面的四元数公式计算。
q0 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) - sin(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //w
q1 = cos(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw) - sin(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw);  //x 绕x轴旋转是roll
q2 = sin(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) + cos(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //y 绕y轴旋转是pitch
q3 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw) + sin(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw);  //z 绕z轴旋转是Yaw

木君之上 · 发表于 2014-2-27 09:49:55

我现在连四元数怎么应用卡尔曼滤波都还不清楚，楼主可以讲一下吗？主要就是那个状态方程和量测方程改怎么建立，状态转换矩阵和测量矩阵怎么确定，这个很头疼

木君之上 · 发表于 2014-2-27 09:52:24

leicheng 发表于 2014-2-26 22:06
还有个问题，计算测量值的四元数，能否不采用高斯牛顿法，而是先通过计算加计和磁强计得到姿态角，再用下面 ...

看来我还得多学习了，高斯牛顿算法还不知道呢，

leicheng · 发表于 2014-2-28 10:10:05

问题出在欧拉角微分方程，解四元数微分方程还是具有优势。

leicheng · 发表于 2014-2-28 10:29:21

木君之上发表于 2014-2-27 09:49
我现在连四元数怎么应用卡尔曼滤波都还不清楚，楼主可以讲一下吗？主要就是那个状态方程和量测方程改怎么建 ...

状态方程和观测方程尽量转化成线性方程的形式，这样就能采用卡尔曼滤波算法估计最佳值，实际中能直接找到对应的线性方程最好，不行的话一般都是采用求一阶偏导的方法，忽略高阶小量，得到满足卡尔曼滤波算法的近似线性方程形式。有时需要一定的数学技巧和一点好运气。

木君之上 · 发表于 2014-2-28 23:38:59

再顶一下，希望有大神来回答，我也在努力看论文，

asha · 发表于 2014-3-2 10:12:38

感觉你搞错了概念，你可能认为状态量和观测量应该完全一样吧？这样是完全错误的。状态量只要和观测量有数学关系就行，最好是可控可观的。我用四元数做状态量的时候，用加速度和磁带输出做观测量。

leicheng · 发表于 2014-3-2 10:28:41

asha 发表于 2014-3-2 10:12
感觉你搞错了概念，你可能认为状态量和观测量应该完全一样吧？这样是完全错误的。状态量只要和观测量有数 ...

请问您建立的观测方程是不是非线性的，直接用加计和磁力计的读数作为Zk,然后对q0、q1、q2、q3分别求一阶偏导，来进行线性化近似处理？
我的方法是不用加计和磁强计的直接观测值，而是通过数学关系转成对应的四元数。状态方程和测量方程此时都是线性的，这样用卡尔曼滤波就很方便了。

nust5042 · 发表于 2015-8-8 21:22:13

求kalman 更新四元数的源码

westloveohyeah · 发表于 2016-3-30 17:39:57

leicheng 发表于 2014-3-2 10:28
请问您建立的观测方程是不是非线性的，直接用加计和磁力计的读数作为Zk,然后对q0、q1、q2、q3分别求一阶 ...

算出四元数很简单，Wahba就行了，但是噪声呢，噪声协方差你能解决？

westloveohyeah · 发表于 2016-3-30 17:40:49

leicheng 发表于 2014-3-2 10:28
请问您建立的观测方程是不是非线性的，直接用加计和磁力计的读数作为Zk,然后对q0、q1、q2、q3分别求一阶 ...

现有方法里用四元数做状态量的可以是可以，但是可观性可控性不强，最好是选择方差稳定的

求Kalman滤波中状态变量选择四元数还是欧拉角问题的解答

阿莫论坛20周年了！感谢大家的支持与爱护！！