里），在这里设置了好久。后来突然想到上篇日志里写的地址移位的问题，查看 MWC 飞控 的 I2C 写入函数，果然地址进去以后先要左移一位，顿时泪奔，0x1E 左移一位是 0x3C，按 这个地址写入瞬间各种正常~
但是在磁阻仪数据读出后，立马又发现了一个问题。飞控板转动 90°，上位机显示实际 的转动是小于 90°的。然后就各种逛论坛啊，各种没思路。于是决定先睡一会儿，困死了， 结果刚躺下立马就有思路了，果然过于专注一个问题思维容易进入死胡同。其实磁阻仪的每 个轴的数据并不是关于 0 对称的，这是一个误差。另一个误差是每个轴的变化量是不同的， 以 X,Y 轴为例，如果以想两轴的数据确定一个点（X,Y），那么如果把飞控板转一圈得到的数 据画出的圆其实是一个椭圆，而且和原点存在偏移。如图：
蓝色点是飞控板在 XY 平面旋转一圈采集到的数据，红色圆圈是用 matlab 拟合得到的圆。 通过 matlab 拟合，我们得到了下图数据： 其中数据从左到右分别为椭圆圆心的坐标（X,Y），然后是椭圆长半轴和短半轴的长度。
所以我们可以利用这些数据对磁阻仪进行校正。
其中椭圆圆心的偏移量分别为磁阻仪 XY 轴数据的偏移量，用原始数据减去这个偏移量即可。 然后由于 Y 轴比 X 轴长，所以把 X 轴数据乘以长半轴和短半轴的商来校正 X 轴数据，这样 XY 轴数据就变成一个没有偏移的圆，通过 atan2 函数就可以求方向角了。
如图，我把飞控板旋转 90°，可以看到上位机读到的数据从 180°左右变到了 270°左右。
但是接下来又存在一个问题，就是这个方法只能在水平面内使用，如果有滚转或俯仰角， 通过磁阻仪计算得到的偏航角就会有很大幅度的变化。然后又是一顿查资料，找到几篇关于 全姿态罗盘的论文，参考上面的把磁阻仪三轴数据变化到平面的公式编入程序中发现，根本 没有什么变化。可能是这个飞控板的传感器安装和论文里所用的安装方位不一样的缘故吧。 然后又是一阵挠头，突然想到 MWC 飞控里应该有相应的程序。于是翻 MWC 的程序，结果 还真找到了。
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z)
上面是 MWC 全姿态罗盘的程序，Est.V.axis 分别是重力向量在机体坐标系下在机体坐标系各 轴的分量（即机体坐标系下重力向量），EstM.V.axis 同理为机体坐标系下磁场的向量。用这 个坐标变换并用 atan2 函数就可算出偏航角。
按照这个思路把程序写出来并实验了一下。
下图中红色为偏航角，黄色为滚转角。可以看到当滚转角发生变化时，偏航角只发生了小幅 的变动，可以看出全姿态罗盘算法起作用了。而偏航角的小幅变动我推测是由于 Z 轴没校正 好的缘故。Z 轴的校正没有用 matlab 拟合，只是简单的找了最大值和最小值算偏移量（由 于只是简单的手动旋转，可能没有找到真正的最大值和最小值），然后用 Z 轴数据的量程和 Y 轴数据的量程做比较得出比例系数。其实如果把三轴数据统计会得到一个椭球体，然后用 matlab 拟合得到椭球体的原点偏移和各轴的长度，相信可以得到更准确的磁阻仪校正。
至于如何用 matlab 拟合，是从论坛上找到的现成的程序，改天把它改成椭球体的拟合。 数据处理是通过串口助手读取数据存到 word 里，然后将文本准换成表格，以空格分列，然 后把各列数据存到 Excel 里，最后用 matlab 读取数据。由于我的 matlab 貌似存在问题，得 用主程序图标以管理员方式运行，进入后把目录设置成拟合程序的目录，然后用 XY=xlsread(‘file’)函数读取数据。
在程序里，今天做的最大的修改就是把均值滤波去掉了。因为发现在姿态解算前积分量只 积分了一次，也就是说就是当前读取传感器的值，所以干脆去掉了。而且传感器最快的读取 速度也就 1KHz，姿态解算的速率是 500Hz，没有必要滤波了。如果以后装到飞机上发现噪 声严重的话再滤波和降低姿态解算速率。
2013 年 2 月 4 日 22:27:33
今天做的主要一件事就是把陀螺仪和磁力计的数据进行融合。因为陀螺仪在很微小的转动 时几乎察觉不不到飞控板的转动，所以需要磁力计的数据进行校正。而磁力计虽然昨天做了 全姿态罗盘的变换，但是在存在俯仰和滚转角时，偏航角任然有扰动，所以又需要陀螺仪来 校正，所以我要把这两个传感器的数据相互融合来取长补短。
其实一开始的想法是通过磁力计解算偏航角，然后把加计和陀螺仪解算的俯仰滚转角和磁 力计解算的偏航角一并转成四元数，用于下次四元数解算。但是实际效果并不是很好。纠结 了一会儿想到一个办法就是把加计和陀螺仪解算的偏航角和磁力计解算的偏航角融合，然后 引入一个积分环节，每次积分的值是陀螺仪解算的偏航角和最终解算偏航角的差，用这个积 分环节去校正由于加计和陀螺仪解算的偏航角的静差（这个偏航角有累积误差）。
这个方法试验了一下效果不错，但是就是存在一个问题。比如由 180°转到 360°（0°）， 由于当磁力计的偏航角到达 360°时，加计和陀螺仪的偏航角未必到达 360°，所以就会出 现一个现象，再稍微顺时针转一下，磁力计的偏航角又变成几度，而加计和陀螺仪的偏航角
还在 300 多度。这样积分环节就会把最终解算的偏航角逆时针转到磁力计的偏航角（大概几 度那里）。这是一个很不好的现象，所以我在这个基础上对算法又进行了改进。由于我们需 要的是陀螺仪数据，所以我们可以直接从角速度入手，如下图：
把角速度计和磁力计数据融合，gz 为角速度数据，乘以负数是和定义的转动方向有关， 我定义的是顺时针转动角度从 0°变到 360°，而 gz 数据正好相反，所以要乘负数。由于每 2ms 调用一次 IMUupdata()，所以 gz*0.002 就是 2ms 转过的角度，对此积分就是实时的角度 把磁力计解算角度和上次解算的偏航角做差得到偏航角的误差，乘以一个系数 0.002（经验 值）再积分用来修正角速度计的累积误差实验表明，角速度计再快速转动时误差较小，但是 转动速度较慢时通过积分几乎得不到偏航角的变化，此时就需要用磁力计校正通过数据融合， 即避免了角速度计的累积误差又可以用角速度计解算的角度校正滚转俯仰时磁力计产生的 误差（从±15°减小到±3°）
这样就很好的避免了上述现象。效果如下：
上图中黄色是滚转角，蓝色是俯仰角（左面图例有误）。当我把飞控板做了三次滚转和三