MWC算法的理解

nbmsse · 发表于 2013-11-1 22:54:24

MWC的理解，也不知对否，有兴趣的朋友参考，或提意见。

mahengyu · 发表于 2013-11-1 23:09:32

自己写的还是？

nbmsse · 发表于 2013-11-1 23:49:01

其它还有研究中。

mahengyu · 发表于 2013-11-2 08:42:15

期待楼主奉献更精彩的文章

pp786702237 · 发表于 2013-11-2 10:29:13

顶一个，感谢分享

wenziheni · 发表于 2013-11-2 12:25:14

顶一下

cai_mouse · 发表于 2013-11-2 21:31:36

很精彩的文章。

zztbb868 · 发表于 2013-11-21 21:53:00

不错，帮助我们理解代码，辛苦了。。。。。。

yangyanzhang1 · 发表于 2013-11-22 09:33:33

很不错的理解。

samo110 · 发表于 2013-11-22 09:51:33

一直纠结于mwc满篇的#ifdef和#endif，楼主这个很好啊

背负拉天的云 · 发表于 2013-11-24 19:58:47

真是雪中送炭啊感谢楼主最近正在纠结MWC的姿态解算部分。。。。

背负拉天的云 · 发表于 2013-11-26 18:18:17

楼主想请教一个关于getEstimatedAttitude()函数的问题：EstG32.V.Z 和EstG.V.Z的关系？

在getEstimatedAttitude()函数中调用了rotateV(&EstG.V,deltaGyroAngle); 对EstG.V向量更新，只用到了角速度参量（deltaGyroAngle[axis] = gyroADC[axis]  * scale;）。。。。（在后面的代码中 EstG.V不见了，很想知道它传递向哪里了？，一下我我的寻找过程）

代码然后就是互补滤波：EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; 只用到了加速度参量accSmooth[axis]

在之后就是浮点转换成整数 EstG32.A[axis] = EstG.A[axis]; //  还是没见EstG.V

最后几行代码了：
// Attitude of the estimated vector
  int32_t sqGZ = sq(EstG32.V.Z);。。。。。。EstG32.V.Z 从哪里冒出来的和EstG.V.Z有什么关系呢？
  int32_t sqGX = sq(EstG32.V.X);
  int32_t sqGY = sq(EstG32.V.Y);
  int32_t sqGX_sqGZ = sqGX + sqGZ;
  float invmagXZ  = InvSqrt(sqGX_sqGZ);
  invG = InvSqrt(sqGX_sqGZ + sqGY);
  angle[ROLL]  = _atan2(EstG32.V.X , EstG32.V.Z);。。。。。。计算角度也是用的EstG32.V.Z 不见EstG.V.Z
  angle[PITCH] = _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);
#if MAG
heading = _atan2(
   EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
   EstM32.V.Y * invG * sqGX_sqGZ  - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y );
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
  #endif

cai_mouse · 发表于 2013-11-30 08:46:16

EstG32.V.Z 和EstG.V.Z是一个联合变量

nbmsse · 发表于 2013-12-1 21:13:22

typedef struct fp_vector {
  float X;
  float Y;
  float Z;
} t_fp_vector_def;

typedef union {
  float A[3];
  t_fp_vector_def V;
} t_fp_vector;
这应该就是回答了。但我的版本里面没有EstG32结构变量。

hugh512 · 发表于 2013-12-2 11:41:12

感谢楼主的辛勤劳作.Mark学习！

nbmsse · 发表于 2013-12-2 21:57:14

上次发送文件后发现有错误，修改的地方件附件图片。

6091820503 · 发表于 2013-12-12 20:11:09

Madgwick的算法貌似也很不错

cyc6125 · 发表于 2013-12-18 20:10:59

顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶顶

mbwhy · 发表于 2013-12-26 18:53:16

求教一下楼主，对MWC的PID理解，P是角度的差，I是角度差的积分，这都好理解，但为什么D是角加速度呢？

qwe2231695 · 发表于 2013-12-26 19:59:15

mbwhy 发表于 2013-12-26 18:53
求教一下楼主，对MWC的PID理解，P是角度的差，I是角度差的积分，这都好理解，但为什么D是角加速度呢？ ...

D是角度的微分，也就是角度变化率，也就是角速度。陀螺仪输出角速度

mbwhy · 发表于 2013-12-26 20:20:29

qwe2231695 发表于 2013-12-26 19:59
D是角度的微分，也就是角度变化率，也就是角速度。陀螺仪输出角速度

贴段代码：
         delta = gyroData[axis] - lastGyro[axis];       //16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
         lastGyro[axis] = gyroData[axis];
         deltaSum = delta1[axis] + delta2[axis] + delta;
         DTerm = ((int32_t) deltaSum * dynD8[axis]) >> 5;

这里 gyroData[axis]不是角速度吗，然后跟前次角速度做差，后面又跟前两次计算的做和，
这不是角速度嘛。。

qwe2231695 · 发表于 2013-12-26 21:02:46

mbwhy 发表于 2013-12-26 20:20
贴段代码：
delta = gyroData[axis] - lastGyro[axis]; //16 bits is ok here, the d ...

哦~ 你怎么把MWC的PID的代码搬过来了~

这个帖子是说角度融合部分。

至于你说的D为什么是陀螺仪再微分。

其实，是这样的：那段PID有兼容某些飞机只有3轴陀螺仪的场合，主控量是角速度。那么PID，的D就要再微分了。

mbwhy · 发表于 2013-12-26 21:30:01

qwe2231695 发表于 2013-12-26 21:02
哦~ 你怎么把MWC的PID的代码搬过来了~ 这个帖子是说角度融合部分。

至于你说的D为什么是陀螺 ...

噢，那难道MWC主要是PI 控制？主要是现在一直觉得姿态结算固然重要，但一个好的PID更会关键啊

qwe2231695 · 发表于 2013-12-26 22:06:01

mbwhy 发表于 2013-12-26 21:30
噢，那难道MWC主要是PI 控制？主要是现在一直觉得姿态结算固然重要，但一个好的PID更会关键啊 ...

是PID的，只是老的飞机只有陀螺仪，得不到角度。所以要用你说的那个代码。。好的pid咳哟你不角度滞后过冲等毛病。但是整体稳定性是姿态决定的。pid决定手感。

mbwhy · 发表于 2013-12-27 08:19:32

qwe2231695 发表于 2013-12-26 22:06
是PID的，只是老的飞机只有陀螺仪，得不到角度。所以要用你说的那个代码。。好的pid咳哟你不角度滞后过 ...

楼层用的是哪种PID呢，能介绍一下吗？

hex6703 · 发表于 2014-1-23 01:09:34

很不错的理解

zwqhzy99 · 发表于 2014-1-23 09:45:17

顶一个，多谢分享了。

茶亦爽 · 发表于 2014-1-23 11:18:32

顶一个，下来看看。

stargrey · 发表于 2014-1-23 23:37:04

楼主急公好义，雪中送炭，感谢

茶亦爽 · 发表于 2014-1-24 00:19:22

多谢分享啦

Eastnorth · 发表于 2014-1-24 07:49:56

这个得支持一下楼主

小乖 · 发表于 2014-2-26 10:20:12

下来看看，谢谢

lubyudf · 发表于 2014-2-26 13:16:11

收藏了..

茶亦爽 · 发表于 2014-2-26 17:13:10

先看看，多谢分享。

cltx · 发表于 2014-2-26 21:34:50

顶一顶！

kingqb · 发表于 2014-3-5 17:37:07

好东西下来看看

LYY · 发表于 2014-6-5 19:16:03

背负拉天的云发表于 2013-11-26 18:18
楼主想请教一个关于getEstimatedAttitude()函数的问题：EstG32.V.Z 和EstG.V.Z的关系？

在getEstimatedAtt ...

你好，请问你弄明白了吗，我最近在看这部分的代码，也有相同的疑惑

LYY · 发表于 2014-6-5 19:17:49

背负拉天的云发表于 2013-11-26 18:18
楼主想请教一个关于getEstimatedAttitude()函数的问题：EstG32.V.Z 和EstG.V.Z的关系？

在getEstimatedAtt ...

EstG.V没有看到它的赋值语句啊？它的值是多少呢?

SMC · 发表于 2014-6-9 04:30:10

nbmsse 发表于 2013-12-2 21:57
上次发送文件后发现有错误，修改的地方件附件图片。

楼主，公式中的v_temp.x是pitch轴上一次的角度吗？用上一次的角度计算最新的角度。
如果是这样的话，那么这个公式依然把磁场起决定作用的YAW数据耦合到了其它两个轴上。

zlpvch · 发表于 2014-6-9 16:14:47

雀一眼

nbmsse · 发表于 2014-6-15 21:52:47

航向角使用了向量ESTG和ESTM的叉乘,为啥得到的新向量的X和Y轴的分量的反正切就是航向角呢? 有明白的说说.

chen4168891 · 发表于 2014-7-23 18:08:47

12楼的问题我也有疑问，不知道谁可以解答一下

xuxiang10 · 发表于 2014-8-22 14:28:27

果断收藏了。。

cntsw · 发表于 2014-9-4 12:42:28

谢谢楼主，很详细

yoyomama513 · 发表于 2014-9-11 09:07:39

谢谢！好资料，正学习呢

yoyomama513 · 发表于 2014-9-11 09:08:01

谢谢！好资料，正学习呢

WZY008 · 发表于 2014-11-25 18:11:38

好东西，谢谢楼主分享！

ゞ從此消失╭ァ · 发表于 2015-4-6 13:39:39

感谢搂住，注释的很到位

forwinry · 发表于 2015-8-18 10:43:07

图里面的坐标系是不是画错了，z轴应该向上吧。而且转动的顺序为什么是先roll再pitch再yaw

qidian · 发表于 2015-10-13 15:43:59

谢谢了，楼主辛苦了

Malo · 发表于 2015-10-29 15:49:29

看了旋转矩阵这块，还是不懂，请教下，可否稍微解释下这段程序起到什么作用？

MWC算法的理解

本帖子中包含更多资源

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

本帖子中包含更多资源