billhsu 发表于 2013-2-7 12:30:27

姿态DCM互补滤波算法求助

网上的DCM互补滤波用的IMU_AHRSupdate好像都是来自一个地方，其中有下面部分理解不了，希望大家帮忙。

// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);      
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;

不太理解上面这个地磁参考方向是怎么算的，*DCM' 是什么意义呢？
还有，为什么需要把y轴分量去掉？ bx = sqrt((hx*hx) + (hy*hy));？

// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);

// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

billhsu 发表于 2013-3-20 23:02:15

地磁参考方向到底为什么这么算呢？

seanwood 发表于 2013-3-21 10:03:10

已更新至我的心得帖{:biggrin:} 去看吧

zhouhuo2010 发表于 2013-3-21 10:17:40

seanwood 发表于 2013-3-21 10:03 static/image/common/back.gif
已更新至我的心得帖 去看吧

能发个链接吗

billhsu 发表于 2013-3-21 22:19:48

seanwood 发表于 2013-3-21 10:03 static/image/common/back.gif
已更新至我的心得帖 去看吧

谢谢！
去看看。

sz_works 发表于 2013-3-28 10:28:56

很好不错

wxm27811 发表于 2013-3-30 17:33:32

不太了解

杨闯 发表于 2013-9-2 20:01:56

seanwood 能给了ＤＣＭ的链接吗

kingjie0210 发表于 2013-12-17 19:19:20

你好，能否解释一下互补滤波体现在什么地方？简单的说一下是怎么个原理，谢谢

brelo 发表于 2015-3-26 21:02:37

zhouhuo2010 发表于 2013-3-21 10:17
能发个链接吗

能发个链接么
页: [1]
查看完整版本: 姿态DCM互补滤波算法求助