Pixhawk学习5.1——传感器标定之加速度计

加速度计作为飞控系统的核心传感器，在Pixhawk中主要充当姿态解算、加速度敏感等任务。由于Pixhawk中采用的为MEMS加速度计，其使用过程中存在着零偏、刻度系数、安装偏角等误差，如果直接使用，即加速度计的输出并非飞机本体的加速度，因此需要提前标定这些误差，在使用时提前补偿掉。
Pixhawk中给出了加速度计的标定模型，在这里稍作总结并提出自己的一些想法。
加速度计标定的程序主要涉及到以下文件：
src/modules/commander/accelermeter_calibration.cpp

加速度计的标定模型如下：
accel_corr = accel_T * (accel_raw - accel_offs)
其中：
accel_corr[3]——为飞机机体系下的加速度值；
accel_T[3][3]——为飞机机体系与飞控坐标系（这里假设加速度计与飞控机壳固连且加计的敏感轴坐标系与飞控机壳坐标系重合）；
accel_raw[3]——为加速度计敏感输出的加速度；
accel_offs[3]——为加速度计敏感轴下的零偏。
该模型的含义为，加速度计敏感到的信息，减去自身零偏之后，再乘以飞控与机体之间的旋转矩阵，可以得到机体系下的加速度。那么标定方法为采用机体系下的加速度值和传感器敏感到的加速度，反算 accel_T[3][3]和 accel_offs[3]。

标定方法采用六位置法，此时机体系下的加速度值为：

 * accel_corr_ref[6][3] = [  g  0  0 ]     // 机头朝上  X 
 *                        | -g  0  0 |     // 机头朝下
 *                        |  0  g  0 |     // 机身左倾  
 *                        |  0 -g  0 |     // 机身右倾  Y
 *                        |  0  0  g |     // 机背朝上
 *                        [  0  0 -g ]     // 机背朝下  Z

那么针对上述每一个位置，都有公式：
accel_corr_ref[i] = accel_T*(accel_raw_ref[i] - accel_offs); i=0~5,对应每个位置。（公式1）
根据上述方法，总共可产生3*6=18个式子，而需要确定 9(accel_T) + 3(accel_offs) = 12个变量。

1、零偏accel_offs标定

零偏accel_offs可根据每个方向的正反两次放置来得到。以i=0和i=1为例（机头向上和向下），两式相加==> 0=accel_T*(accel_raw_ref[0] + accel_raw_ref[1] - 2accel_offs)
由于i=0和i=1两种情况时g主要在X轴上，Y和Z基本为零。因此：
accel_offs(X) = 1/2(accel_raw_ref[0] + accel_raw_ref[1])。accel_offs(Y) 和 accel_offs(Z) 一次类推。

2、零偏accel_offs标定
标定好accel_offs之后，选取公式1中i=0，i=2，i=4项，则有以下公式：

通过简单矩阵运算就可以得到accel_T。