刚煜 发表于 2015-1-17 10:50:05

惯性导航算法中四元微分方程中的问题

最近在MFC中搭建了OPENGL环境打算写了姿态解算的小程序，阅读了《捷联惯导算法心得》，对我这种新手很有帮助，那么问题来了

在帖子中，四元数更新算法是这样的

然而，在下面的代码实现部分是这样的

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

我们那个公式中得出来的q(n+1)是指后一个时刻的四元数，假设用这个代码来实现的话，q0算出来是没问题，但是计算q1的时候，这个时候的q0就已经是更新后的q0了，即q（0+T）,那么这么算下来q2和q3都不是严格意义上的q2和q3了，

难道这么算的话对整个姿态解算的影响不怎么大么？

我翻开我大学同学做四轴的代码，发现他也是这样写的，貌似也能飞的样子。

大家怎么看

qs6361036 发表于 2015-1-17 11:13:54

四元素的公式有多种表达方式 ，只是变换了一下 ！

刚煜 发表于 2015-1-17 12:10:07

qs6361036 发表于 2015-1-17 11:13
四元素的公式有多种表达方式 ，只是变换了一下 ！

也许是我没有表达明白，我的意思是问如果代码这么写的话，那么计算出来的就不是这个公式想表达的结果了

kingway00 发表于 2015-1-17 18:53:37

楼主，你的理解是正确的。这个代码确实有这个问题，只是很多人直接拿来用，也不问为什么

NJ8888 发表于 2015-1-17 18:59:09

刚煜 发表于 2015-1-17 12:10
也许是我没有表达明白，我的意思是问如果代码这么写的话，那么计算出来的就不是这个公式想表达的结果了 ...

他算完第一条q0,在第二条中q1右边出现的q0，其实就是q（0+1）啊，语句是顺序执行的，他用q0地址空间放q0的新数据

刚煜 发表于 2015-1-17 20:13:57

kingway00 发表于 2015-1-17 18:53
楼主，你的理解是正确的。这个代码确实有这个问题，只是很多人直接拿来用，也不问为什么 ...

嗯 看来就是这样，也有可能按照顺序这么算其实与并行计算出来的结果也相差不大，有待考证

刚煜 发表于 2015-1-17 20:15:34

NJ8888 发表于 2015-1-17 18:59
他算完第一条q0,在第二条中q1右边出现的q0，其实就是q（0+1）啊，语句是顺序执行的，他用q0地址空间放q0 ...

嗯是的，所以这个q0已经是更新后的了q0，就是后一时刻的q0，用这个后一时刻的q0来算后一时刻的q1就不是上面那个公式想表达的：用前一时刻的q0来算出后一时刻的q1

kingway00 发表于 2015-1-17 22:27:57

刚煜 发表于 2015-1-17 20:13
嗯 看来就是这样，也有可能按照顺序这么算其实与并行计算出来的结果也相差不大，有待考证 ...

非也。你把互补滤波去掉，你就知道差别了。之所以你同学的四轴能飞，是因为互补滤波帮他把各种误差滤掉了

chxaitz 发表于 2015-1-18 00:16:13

不懂帮顶～

刚煜 发表于 2015-1-18 09:54:48

kingway00 发表于 2015-1-17 22:27
非也。你把互补滤波去掉，你就知道差别了。之所以你同学的四轴能飞，是因为互补滤波帮他把各种误差滤掉了 ...

原来如此，我去学习学习互补滤波

2013的弹子球 发表于 2015-1-18 19:26:37

角速度方向定义不一样所以由正负差别。一样的，一阶龙格库塔发跟新姿态就是这一时刻的量等于上一时刻量加上微分量

2013的弹子球 发表于 2015-1-18 19:27:48

。。我理解错了。。从来就没发现这问题。。。

laylovesb1314 发表于 2016-1-10 01:29:23

楼主问了个号问题，我刚好也遇到这个疑惑刚好看到了！嘻嘻！

dreamer2016 发表于 2016-1-12 14:08:41

mark 学习中
页: [1]
查看完整版本: 惯性导航算法中四元微分方程中的问题