姿态DCM互补滤波算法求助

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

网上的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;

  不太理解上面这个地磁参考方向是怎么算的，[mx my mz]*DCM[q]' 是什么意义呢？
还有，为什么需要把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

已更新至我的心得帖

去看吧

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

seanwood 发表于 2013-3-21 10:03
已更新至我的心得帖去看吧

能发个链接吗

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

seanwood 发表于 2013-3-21 10:03
已更新至我的心得帖去看吧

谢谢！
去看看。

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
能发个链接吗

能发个链接么

姿态DCM互补滤波算法求助

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