尼玛啊，看错程序差点出大问题。

zouyf12

先声明一点， 这个帖子的用意不是在说谁对谁错，而是把我发现的一个问题和大家讨论，如果说的是对的，可以给大家提个醒， 如果错了，也欢迎大家批评。

http://www.amobbs.com/forum.php?mod=viewthread&tid=5492189&extra=page%3D1%26filter%3Ddigest%26digest%3D1%26digest%3D1

这篇我相信很多人都看过，  但是在今天下午之前，包括我在内都没人发现这里面附带的程序有问题（通过回帖来看，如有遗漏欢迎批评）。具体看下面

先看看公式：


再看看程序：
        // integrate quaternion rate and normalise

        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;  

有问题吗？ 思考10秒钟后有了答案再往下看。。。。


第一次看我觉得是正确的，   实验上的数据也基本上能反应姿态的状态，更重要的是，有网友已经用这个程序飞起来了。
但是！

图中的公式是每一个元素N+1时刻的数值 都和 N 时刻 4个元素 有关。
然而，如果按照程序的写法的话， 只有Q0的数值是正确更新的，  当执行到Q1更新时， 公式里面的那个Q0 已经是N+1时刻的Q0了。 依次类推， Q2 和 Q3就会错的更离谱。

brb2010

好像是有些问题

qwe2231695

我用这个飞得比较稳，波形也没有滞后。让我想想::>_<::

zouyf12

qwe2231695 发表于 2013-5-15 19:28
我用这个飞得比较稳，波形也没有滞后。让我想想::>_

我只是从公式上面分析， 这个程序这样写肯定和公式有出入，  但是实际我运算过， 由于Q（N+1） 和Q（N） 一般差得不是很大，  所以计算结果上也可以用，带来的误差和陀螺仪本身漂移误差差不多，所以滤波做的好的话会被滤掉。

qwe2231695

我改成了：
  qq0=q0;qq1=q1;qq2=q2;qq3=q3;

  q0 = qq0 + (-qq1*gx - qq2*gy - qq3*gz)*halfT;
  q1 = qq1 + (qq0*gx + qq2*gz - qq3*gy)*halfT;
  q2 = qq2 + (qq0*gy - qq1*gz + qq3*gx)*halfT;
  q3 = qq3 + (qq0*gz + qq1*gy - qq2*gx)*halfT;

一样还是可以飞，肉眼看不出差别

zouyf12

qwe2231695 发表于 2013-5-15 20:05
我改成了：
  qq0=q0;qq1=q1;qq2=q2;qq3=q3;

恩，  不过还是和公式一致的好，   其实我也不是看程序才发现错的，而是偶然发现的，    之前我实验的效果如你所说基本没有差别，  

zouyf12

qwe2231695 发表于 2013-5-15 20:05
我改成了：
  qq0=q0;qq1=q1;qq2=q2;qq3=q3;

另外请教一下前辈， 你的程序里滤波是用什么方法？

qwe2231695

zouyf12 发表于 2013-5-15 20:12
另外请教一下前辈， 你的程序里滤波是用什么方法？

就是这一段代码 ，加上PID。 还没有研究卡尔曼。

zouyf12

qwe2231695 发表于 2013-5-15 21:03
就是这一段代码 ，加上PID。 还没有研究卡尔曼。

     嗯， 那就是由于有那个用加速度计向量来叉乘矫正的缘故，所以把这部分带来的偏差当成漂移给滤掉了，  我之前由于不懂这个叉乘的原理，所以删了这部分代码，直接看四元数的输出，结果基本上2-3秒就可以飘一度。  但是我还想当年用ENC-03都没有这么惨，  现在想想可能是这个缘故

jmp2002911911

高啊，严谨，果然是技术达人啊

Puppey

我的理解是1Ms的解算周期的话，Q2 Q3 Q4 基本和之前是一样的。所以公式也是可以的。但是还是建议用5楼的~

mrcbf

大神啊   又学到了