尼玛啊，看错程序差点出大问题。
先声明一点， 这个帖子的用意不是在说谁对谁错，而是把我发现的一个问题和大家讨论，如果说的是对的，可以给大家提个醒， 如果错了，也欢迎大家批评。http://www.amobbs.com/forum.php?mod=viewthread&tid=5492189&extra=page%3D1%26filter%3Ddigest%26digest%3D1%26digest%3D1
这篇我相信很多人都看过，但是在今天下午之前，包括我在内都没人发现这里面附带的程序有问题（通过回帖来看，如有遗漏欢迎批评）。具体看下面
先看看公式：
http://cache.ourdev.cn/new2012/forum/201208/16/150900wu72xpzv87uwx86r.jpg
再看看程序：
// 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就会错的更离谱。
好像是有些问题 我用这个飞得比较稳，波形也没有滞后。让我想想::>_<:: qwe2231695 发表于 2013-5-15 19:28 static/image/common/back.gif
我用这个飞得比较稳，波形也没有滞后。让我想想::>_
我只是从公式上面分析， 这个程序这样写肯定和公式有出入，但是实际我运算过， 由于Q（N+1） 和Q（N） 一般差得不是很大，所以计算结果上也可以用，带来的误差和陀螺仪本身漂移误差差不多，所以滤波做的好的话会被滤掉。 我改成了：
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;
一样还是可以飞，肉眼看不出差别 qwe2231695 发表于 2013-5-15 20:05 static/image/common/back.gif
我改成了：
qq0=q0;qq1=q1;qq2=q2;qq3=q3;
恩，不过还是和公式一致的好， 其实我也不是看程序才发现错的，而是偶然发现的， 之前我实验的效果如你所说基本没有差别， qwe2231695 发表于 2013-5-15 20:05 static/image/common/back.gif
我改成了：
qq0=q0;qq1=q1;qq2=q2;qq3=q3;
另外请教一下前辈， 你的程序里滤波是用什么方法？ zouyf12 发表于 2013-5-15 20:12 static/image/common/back.gif
另外请教一下前辈， 你的程序里滤波是用什么方法？
就是这一段代码 ，加上PID。 还没有研究卡尔曼。 qwe2231695 发表于 2013-5-15 21:03 static/image/common/back.gif
就是这一段代码 ，加上PID。 还没有研究卡尔曼。
嗯， 那就是由于有那个用加速度计向量来叉乘矫正的缘故，所以把这部分带来的偏差当成漂移给滤掉了，我之前由于不懂这个叉乘的原理，所以删了这部分代码，直接看四元数的输出，结果基本上2-3秒就可以飘一度。但是我还想当年用ENC-03都没有这么惨，现在想想可能是这个缘故 高啊，严谨，果然是技术达人啊 我的理解是1Ms的解算周期的话，Q2 Q3 Q4 基本和之前是一样的。所以公式也是可以的。但是还是建议用5楼的~ 大神啊 又学到了{:smile:}
页:
[1]