树莓派采集MPU9250运行AHRS进行姿态解算
文章目录
-
- 1.几种概念的区分
- 2.消费级IMU的AHRS
- 3.树莓派玩转MPU9250
-
- 3.1树莓派配置
- 3.2在树莓派中移植MPU9250库
- 3.3使用MPU9250库
- 4.校准
-
- 4.1IMU误差模型
- 5.待续
1.几种概念的区分
IMU：全称Inertial measurement unit，惯性测量单元，由三个正交的加速度敏感元件和三个正交的角速度敏感元件组成，该模块只输出三轴加速度和三轴角速度，其中输出三轴加速度的输出量里包含了重力加速度g。IMU从精密程度可以分为微机电（MEMS）、光纤、激光等。
SINS：全称Strapdown inertial navigation system，捷联惯性导航系统（该定义相对于平台惯性导航系统）。IMU的敏感轴与载具主轴固联对齐。SINS由IMU与惯导计算机里运行的捷联惯性导航算法组成（下图显示了局部坐标系下的SINS算法结构）。该系统输入IMU的输出的加速度和角速度，输出姿态、速度、位置。
AHRS：全称Attitude and Heading Reference System，航姿参考系统。从理论上说，给定初始姿态、速度、位置，由SINS系统可以得到以后每一时刻的姿态、速度、位置。但是由于SINS算法里存在三个积分（在上图中，Attitude Computation也有一个积分），积分效果会不断积累角速度与角速度的误差，导致状态输出会很快发散，经过的积分环节越多，状态发散的越快。因此IMU的精密程度限制了SINS的使用，当使用低成本的MEMS时，有SINS得到的速度和位置就不再具有参考意义，但是姿态还是可以得到一定的保证。因此，AHRS通常是由低成本的MEMS IMU和AHRS算法组成。由于，加速度可以感知重力g，其可以用于校正横滚和俯仰角，通常AHRS会加上一个三轴磁力计，感知地磁来校正艏向。AHRS系统输入三轴加速度、三轴角速度、三轴磁场强，输出艏向角、横滚角、俯仰角。
VRS：全称Vertical reference system，垂直参考系统。AHRS的简化版，其没有三轴磁力计，无法校正艏向角，因此只能输出横滚角和俯仰角。
INS：全称Integrated navigation system，组合导航系统（也可是Inertial navigation system的缩写，但是意义就不同了）。上面说到，SINS的姿态、速度和位置误差会随时间积累，因此需要外部辅助传感器来进行校正和信息融合。如：加速度计可以校正横滚、俯仰；磁力计可以校正艏向；GNSS可以校正位置；里程计合DVL可以校正速度；USBL可以校正水下定位位置；地形匹配辅助导航；视觉辅助等。对于处理多源信息融合，最经典的算法是卡尔曼滤波以及延申出的扩展卡尔曼、无迹卡尔曼等等。组合导航系统由IMU和辅助传感器硬件组成，和组合导航算法组成。其中，对于长航程水下航行器（潜艇）来说，组合导航系统是至关重要的设备。
2.消费级IMU的AHRS
低成本的AHRS出现使得每个人都可以玩无人机和平衡车。最常见的9轴模块无非就是MPU9250（本文主要使用该IMU），diy小四轴的必备器件，而AHRS算法在网上也是一搜一大堆：Mahony，Madgwick，EKF，ESKF等。已有文章将互补滤波Mahony讲的很透彻，且直接贴上了源码，但是有人直接读取MPU9250的9轴数据输入到滤波算法中也不一定能得到好的效果，尤其是艏向。静止时艏向会稳定，旋转一周时，0-360°分步不均匀。那是因为IMU在使用时首先需要校准，并且校准是非常重要的。
IMU会存在系统误差：零偏误差，比例误差，非线性误差，正负比例不对称，死区误差，非正交误差，不对中误差等。如果是精密IMU这些系统误差在出厂前通过专业的测量试验来补偿掉大部分，但是低成本IMU则不会。校准就是出厂后，通过采集原始数据，自己通过一定的算法计算出补偿参数。
当然IMU还会存在噪声：偏差重复性误差，偏差飘移（温漂），比例系数不稳定，白噪声等。这些误差会带入到积分计算中，因此需要信息融合来得到状态的最优估计。
3.树莓派玩转MPU9250
3.1树莓派配置
- 树莓派安装Bcm2835库，参考这篇博客；
- Bcm2835的IIC基础操作，参考这篇博客，如果连线都正常，可以读出MPU9250的地址为0x68；
- AHRS算法使用矩阵运算比较方便，需要使用EigenC++矩阵库，安装和使用参考该教程；
- 本MPU9250库还使用了nlohmann/json保存校准参数，因此需要了解，参考该教程。
3.2在树莓派中移植MPU9250库
网上当然有很好的MPU9250库，我觉得最优秀的应该是这个Arduino中的MPU9250库，该库中的AHRS算法有：1.纯四元素更新算法，无重力校正过程（no_filter）；2.mahony；3.madgwick。还包含了水平去偏置校正函数（calibrate_acc_gyro_impl），校正加速度计和陀螺仪的偏置。还有磁力计的校正函数（calibrate_mag_impl），但是我感觉该函数有些缺陷。
mahony参考这个博客；
madgwick参考论文《An efficient orientation filter for inertial and inertial/magnetic sensor arrays》，大佬直接把C代码实现放在附录了，真大佬。
EKF原理参考论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》，代码参考这个大佬里的EkfFilter.m
将上库移植到树莓派中，当然首先要将IIC通信改为Bcm2835的IIC通信函数。然后对上库做了如下深入理解：
- 海洋机器人里通常的附体坐标系为前x-右y-下z，MPU9250模块PCB上标示了xyz轴的方向，通常把标示方向与机器人附体系主轴方向对齐。该标示方向的x轴与加速度计（陀螺仪）的x轴方向一致，y轴和z轴方向相反。根据磁力计与加计的相对方向关系，可知磁力计三轴与标示方向的姿态关系。最终MPU9250::update里向QuaternionFilter::update()传入的形参为(-ax, ay, az, gx, -gy, -gz, my, -mx, mz)；
- 库中提供了对加计和陀螺仪偏置校准的函数calibrate_acc_gyro_impl()，其实现过程是MPU水平静止放置，采集一段时间数据，将通道读数取个平均值视为偏置量，然后写入到芯片中，以后的数据输出就是减去改偏置量的读数。如果在偏置校准的时候芯片没有水平放置，那么重力就不只是在加计的Z轴，还分散到X和Y轴。此时Z轴的重力被分散的部分，和XY分散的部分都会视作偏置量，那么校准就引入了更大的误差，其最终效果就是AHRS算法解算出的姿态是相对于校准时放置姿态的增量，而不是相对于水平的增量。
- 库中提供了对地磁场的校准函数calibrate_mag_impl().如果用一个理想的三轴磁力计在理想的地磁环境下同一点旋转，将三轴通道的读数绘制出来将出现一个在球心在原点的球形。但是由于硬磁和软磁的和器件本身的偏置、不对称、非线性，将会出现一个球心不再原点的椭球。该库磁力计校准函数记录一段时间各通道的读数，分别从中找出三轴读数的最大值和最小值，然后分别求出球心偏置和主轴比例失调因子。关键是不能保证在记录的一段时间内，如何旋转才能覆盖记录下三轴读数的最大值和最小值，不然校准就会有问题。这个比加计和陀螺仪偏置校准条件要苛刻。
- 加计和陀螺仪偏置校准的函数只校准了偏置，可以视为一个粗校准。地磁场的校准函数条件要求苛刻。都需要寻求额外的校准方法，具体方法见“校准”。
- 库中的mahony的AHRS算法，只是用了重力来校准姿态，我参考这个博客将磁力计也组合进来，同时对姿态和艏向进行补偿。mahony算法大致是使用向量叉积来描述理论重力（磁场）（由角速度四元素更新的）和实际重力（磁场）（由加计或磁力计测量出来的）的误差，然后借助控制中的PI来组合两者，补偿四元素更新的姿态。但是mahony算法，我测试起来有问题，但艏向缓慢的穿过360度时，算法会发散。如果比较快的通过360度，或动态比较剧烈就不会发散，暂时原因不明
//acc[mg], gyro[deg/s], mag [mG]
bool mahony(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
float recipNorm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez; //error terms
static float ix = 0.0, iy = 0.0, iz = 0.0; //integral feedback terms
float tmp;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q[0]*q[0];
float q0q1 = q[0]*q[1];
float q0q2 = q[0]*q[2];
float q0q3 = q[0]*q[3];
float q1q1 = q[1]*q[1];
float q1q2 = q[1]*q[2];
float q1q3 = q[1]*q[3];
float q2q2 = q[2]*q[2];
float q2q3 = q[2]*q[3];
float q3q3 = q[3]*q[3];
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
tmp = ax * ax + ay * ay + az * az;
if (tmp == 0.f) return false;
// Normalise accelerometer (assumed to measure the direction of gravity in body frame)
recipNorm = 1.0 / sqrt(tmp);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
tmp = mx * mx + my * my + mz * mz;
if(tmp == 0.f) return false;
recipNorm = 1.0 / sqrt(tmp);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// compute reference direction of magnetic field
//这里计算得到的是地磁计在理论地磁坐标系下的机体上三个轴的分量
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计算的是当前航向角和磁北的夹角，也就是北天东坐标下的航向角
//当罗盘水平旋转的时候，航向角在0-360之间变化
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
//地磁计在n系下磁向量转换到b系下，反向使用DCM得到
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);
// Estimated direction of gravity in the body frame (factor of two divided out)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
// Error is cross product between estimated and measured direction of gravity in body frame
// (half the actual magnitude)
ex = (ay * vz - az * vy) + (my*wz - mz*wy);
ey = (az * vx - ax * vz) + (mz*wx - mx*wz);
ez = (ax * vy - ay * vx) + (mx*my - my*wx);
// Compute and apply to gyro term the integral feedback, if enabled
if (Ki > 0.0f) {
ix += Ki * ex * deltaT; // integral error scaled by Ki
iy += Ki * ey * deltaT;
iz += Ki * ez * deltaT;
gx += ix; // apply integral feedback
gy += iy;
gz += iz;
}
// Apply proportional feedback to gyro term
gx += Kp * ex;
gy += Kp * ey;
gz += Kp * ez;
// Integrate rate of change of quaternion, q cross gyro term
deltaT = 0.5 * deltaT;
gx *= deltaT; // pre-multiply common factors
gy *= deltaT;
gz *= deltaT;
q[0] += (-q[1] * gx - q[2] * gy - q[3] * gz);
q[1] += (q[0] * gx + q[2] * gz - q[3] * gy);
q[2] += (q[0] * gy - q[1] * gz + q[3] * gx);
q[3] += (q[0] * gz + q[1] * gy - q[2] * gx);
// renormalise quaternion
recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] = q[0] * recipNorm;
q[1] = q[1] * recipNorm;
q[2] = q[2] * recipNorm;
q[3] = q[3] * recipNorm;
return true;
}
- 库中madgwick算法参考论文《An efficient orientation filter for inertial and inertial/magnetic sensor arrays》，论文后直接给出了C代码的实现。但是原库中未将陀螺仪的偏差估计加入进去，我尝试加入其中，但是发现会发散，因此又注释了。可能是参数等调整，或者其他有问题，没时间继续研究了。
bool madgwick(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3]; // short name local variable for readability
double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
double hx, hy;
//double w_err_x, w_err_y, w_err_z;
//static double w_bx = 0.0, w_by = 0.0, w_bz = 0.0;
double _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Normalise accelerometer measurement
double a_norm = ax * ax + ay * ay + az * az;
if (a_norm == 0.) return false; // handle NaN
recipNorm = 1.0 / sqrt(a_norm);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
double m_norm = mx * mx + my * my + mz * mz;
if (m_norm == 0.) return false; // handle NaN
recipNorm = 1.0 / sqrt(m_norm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = 1.0 / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
/*
//Compute angular estimated direction of the gyroscope error
w_err_x = _2q0 * s1 - _2q1 * s0 - _2q2 * s3 + _2q3 * s2;
w_err_y = _2q0 * s2 + _2q1 * s3 - _2q2 * s0 - _2q3 * s1;
w_err_z = _2q0 * s3 - _2q1 * s2 + _2q2 * s1 - _2q3 * s0;
//Compute and remove the gyroscope baises
w_bx += w_err_x * deltaT * zeta;
w_by += w_err_y * deltaT * zeta;
w_bz += w_err_z * deltaT * zeta;
gx -= w_bx;
gy -= w_by;
gz -= w_bz;
*/
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * deltaT;
q1 += qDot2 * deltaT;
q2 += qDot3 * deltaT;
q3 += qDot4 * deltaT;
// Normalise quaternion
recipNorm = 1.0 / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
return true;
}
- 在原有的框架下新增EKF算法，算法参考论文《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU》，最后感觉EKF算法比madgwick的动态性能好点。
bool ekf(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q) {
float tmp;
//auxiliary variables to reduce number of repeated operations
//float q0q0 = q[0]*q[0];
float q0q1 = q[0]*q[1];
float q0q2 = q[0]*q[2];
float q0q3 = q[0]*q[3];
float q1q1 = q[1]*q[1];
float q1q2 = q[1]*q[2];
float q1q3 = q[1]*q[3];
float q2q2 = q[2]*q[2];
float q2q3 = q[2]*q[3];
float q3q3 = q[3]*q[3];
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
Vector3d Za(ax,ay,az);
tmp = Za.norm();
if (tmp == 0.f) return false;
// Normalise accelerometer (assumed to measure the direction of gravity in body frame)
Za = Za/tmp;
Vector3d Zm(mx,my,mz);
tmp = Zm.norm();
if(tmp == 0.f) return false;
Zm = Zm/tmp;
//compute reference direction of magnetic field
//这里计算得到的是地磁计在理论地磁坐标系下的机体上三个轴的分量
float hx = 2*Zm(0)*(0.5 - q2q2 - q3q3) + 2*Zm(1)*(q1q2 - q0q3) + 2*Zm(2)*(q1q3 + q0q2);
float hy = 2*Zm(0)*(q1q2 + q0q3) + 2*Zm(1)*(0.5 - q1q1 - q3q3) + 2*Zm(2)*(q2q3 - q0q1);
float hz = 2*Zm(0)*(q1q3 - q0q2) + 2*Zm(1)*(q2q3 + q0q1) + 2*Zm(2)*(0.5 - q1q1 - q2q2);
//bx计算的是当前航向角和磁北的夹角，也就是北天东坐标下的航向角
//当罗盘水平旋转的时候，航向角在0-360之间变化
float bx = sqrt((hx*hx) + (hy*hy));
float bz = hz;
Vector4d x(q[0],q[1],q[2],q[3]);
Matrix4d Ak;
Ak << 0, -gx, -gy, -gz,
gx, 0, gz, -gy,
gy, -gz, 0, gx,
gz, gy, -gx, 0;
Ak *= 0.5f * deltaT;
Ak += Matrix4d::Identity();
x = Ak * x;
x = x/x.norm();
Matrix4d Ppre = Ak * P * Ak.transpose() + Q;
Vector3d R1(x(0)*x(0)+x(1)*x(1)-x(2)*x(2)-x(3)*x(3),
2*(x(1)*x(2)-x(0)*x(3)),
2*(x(1)*x(3)+x(0)*x(2)));
Vector3d R3(2*(x(1)*x(3)-x(0)*x(2)),
2*(x(0)*x(1)+x(2)*x(3)),
x(0)*x(0)-x(1)*x(1)-x(2)*x(2)+x(3)*x(3));
MatrixXd J1(3,4),J3(3,4);
J1 << x(0), x(1), -x(2), -x(3),
-x(3), x(2), x(1), -x(0),
x(2), x(3), x(0), x(1);
J1 = 2*J1;
J3 << -x(2), x(3), -x(0), x(1),
x(1), x(0), x(3), x(2),
x(0), -x(1), -x(2), x(3);
J3 = 2*J3;
MatrixXd Kk(4,3);
Kk = Ppre*J3.transpose()*((J3*Ppre*J3.transpose()+Ra).inverse());
Vector4d qe1 = Kk*(Za - R3);
Matrix4d Pk1 = (Matrix4d::Identity()-Kk*J3)*Ppre;
Vector3d h2 = bx * R1 + bz * R3;
MatrixXd Hk2(3,4);
Hk2 = bx * J1 + bz * J3;
Kk = Ppre*Hk2.transpose()*((Hk2*Ppre*Hk2.transpose()+Rm).inverse());
Vector4d qe2 = Kk*(Zm - h2);
P = (Matrix4d::Identity()-Kk*Hk2)*Pk1;
x = x + qe1 + qe2;
// renormalise quaternion
x = x/x.norm();
q[0] = x(0);
q[1] = x(1);
q[2] = x(2);
q[3] = x(3);
return true;
}
- 在原有的框架下新增ESKF算法，算法参考论文《Quaternion kinematics for the error-state Kalman filter》，但最后试验起来发散，不知道啥原因。
bool eskf(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float* q){
float tmp;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
Vector3d Za(ax,ay,az);
tmp = Za.norm();
if (tmp == 0.f) return false;
// Normalise accelerometer (assumed to measure the direction of gravity in body frame)
Za = Za/tmp;
Vector3d Zm(mx,my,mz);
tmp = Zm.norm();
if(tmp == 0.f) return false;
Zm = Zm/tmp;
//compute reference direction of magnetic field
//这里计算得到的是地磁计在理论地磁坐标系下的机体上三个轴的分量
float hx = 2*Zm(0)*(0.5 - q2q2 - q3q3) + 2*Zm(1)*(q1q2 - q0q3) + 2*Zm(2)*(q1q3 + q0q2);
float hy = 2*Zm(0)*(q1q2 + q0q3) + 2*Zm(1)*(0.5 - q1q1 - q3q3) + 2*Zm(2)*(q2q3 - q0q1);
float hz = 2*Zm(0)*(q1q3 - q0q2) + 2*Zm(1)*(q2q3 + q0q1) + 2*Zm(2)*(0.5 - q1q1 - q2q2);
//bx计算的是当前航向角和磁北的夹角，也就是北天东坐标下的航向角
//当罗盘水平旋转的时候，航向角在0-360之间变化
float bx = sqrt((hx*hx) + (hy*hy));
float bz = hz;
//误差状态卡尔曼滤波，名义状态更新:
gx = (gx-Bg(0))*deltaT;
gy = (gy-Bg(1))*deltaT;
gz = (gz-Bg(2))*deltaT;
Vector4d x(q[0],q[1],q[2],q[3]);
Matrix4d Ak;
Ak << 1, -gx/2, -gy/2, -gz/2,
gx/2, 1, gz/2, -gy/2,
gy/2, -gz/2, 1, gx/2,
gz/2, gy/2, -gx/2, 1;
x = Ak * x;
x = x/x.norm();
//误差状态卡尔曼滤波，预测过程：
Vector3d u(gx,gy,gz);
float delta_theta = u.norm();
u = u/delta_theta;
Matrix3d R_u_delta_theta = Matrix3d::Identity() - skew_symmetric(u)*sin(delta_theta) + skew_symmetric(u)*skew_symmetric(u)*(1-cos(delta_theta));
Matrix6d F_delta_X;
F_delta_X << R_u_delta_theta, -Matrix3d::Identity()*deltaT,
Matrix3d::Zero(),Matrix3d::Identity();
Matrix6d P_k = F_delta_X*Pk1*F_delta_X.transpose()+Q_delta_X;
//误差状态卡尔曼滤波，校正过程：
Vector3d R1(x(0)*x(0)+x(1)*x(1)-x(2)*x(2)-x(3)*x(3),
2*(x(1)*x(2)-x(0)*x(3)),
2*(x(1)*x(3)+x(0)*x(2)));
Vector3d R3(2*(x(1)*x(3)-x(0)*x(2)),
2*(x(0)*x(1)+x(2)*x(3)),
x(0)*x(0)-x(1)*x(1)-x(2)*x(2)+x(3)*x(3));
MatrixXd J1(3,4),J3(3,4);
J1 << x(0), x(1), -x(2), -x(3),
-x(3), x(2), x(1), -x(0),
x(2), x(3), x(0), x(1);
J1 = 2*J1;
J3 << -x(2), x(3), -x(0), x(1),
x(1), x(0), x(3), x(2),
x(0), -x(1), -x(2), x(3);
J3 = 2*J3;
MatrixXd H_x(6,7);
H_x << J3, Matrix3d::Zero(),
bx*J1+bz*J3,Matrix3d::Zero();
MatrixXd Q_delta_theta(4,3);
Q_delta_theta << -x(1), -x(2), -x(3),
x(0), -x(3), x(2),
x(3), x(0), -x(1),
-x(2), x(1), x(0);
Q_delta_theta = 0.5*Q_delta_theta;
MatrixXd X_delta_x(7,6);
X_delta_x << Q_delta_theta, MatrixXd::Zero(4,3),
Matrix3d::Zero(), Matrix3d::Identity();
Matrix6d Hk = H_x * X_delta_x;
Matrix6d Kk = P_k*Hk.transpose()*((Hk*P_k*Hk.transpose()+Rk).inverse());
Vector6d z;
z.segment(0,3) = Za;
z.segment(3,3) = Zm;
Vector3d h2 = bx * R1 + bz * R3;
Vector6d h;
h.segment(0,3) = R1;
h.segment(3,3) = h2;
Vector6d delta_X_hat = Kk*(z-h);
P_k = (Matrix6d::Identity()-Kk*Hk)*P_k;
//误差状态卡尔曼滤波，向名义状态注入误差校正
Vector4d delta_q;
delta_q(0) = 1;
delta_q(1,3) = delta_X_hat.head(3)/2;
Vector4d Q2 = quaternProd(x,delta_q);
Q2 = Q2/Q2.norm();
Bg = Bg+delta_X_hat.tail(3);
q[0] = Q2(0);
q[1] = Q2(1);
q[2] = Q2(2);
q[3] = Q2(3);
//误差状态卡尔曼滤波，滤波器重置
Matrix6d G;
G << Matrix3d::Identity()-skew_symmetric(-delta_X_hat.tail(3)/2), Matrix3d::Zero(),
Matrix3d::Zero(), Matrix3d::Identity();
P_k = G*P_k*P_k.transpose();
}
- 待续。。。
最后移植好的库见github。
3.3使用MPU9250库
源码中提供了三个例程：
1.recordData.cpp记录九轴原始数据至csv，用于CalMPUData.m校准加计和陀螺仪；
2.recordMagData.cpp记录三轴磁数据至csv，用于magCal.m校准磁力计；
3.testMain.cpp采集实时数据（需要把上两步的校准参数修改到该cpp文件中），进行姿态解算。
4.校准
原理：论文《A Robust and Easy to implement method for imu calibration without External Equipments》，
这个视频里有AHRS数据融合方法和磁力计校准方法。
参考代码： https://github.com/shenshikexmu/IMUCalibration-Gesture，大佬的CSDN博客
我修改后的校准程序：这里。只需先根据原理来采集原始数据，再CalMPUData.m校准加计和陀螺仪。再magCal.m校准磁力计。收集原始数据使用我提供的例程。我使用的是Matlab2020b，运行magCal.m需要安装Sensor Fusion and Tracking Toolbox。
4.1IMU误差模型
IMU的误差来主要来自于三部分，包括噪声(Bias and Noise)、尺度因子(Scale errors)和轴偏差(Axis misalignments)。加速度计和陀螺仪的测量模型：
a B = T a K a ( a S + b a + v a ) a^B=T^aK^a(a^S+b^a+v^a) aB=TaKa(aS+ba+va)
w B = T g K g ( w S + b g + v g ) w^B=T^gK^g(w^S+b^g+v^g) wB=TgKg(wS+bg+vg)
其中上标 a a a标示加速度计， g g g标示陀螺仪， B B B标示正交的参考坐标系， S S S标示非正交的选准坐标系。 T T T表示轴偏差的变换矩阵， K K K表示尺度因子， a S a^S aS， w S w^S wS表示真值， b b b， v v v分别表示Bias和白噪声。其中 T , K , b T,K,b T,K,b就是校正要得到的矩阵或向量。
并不是矩阵的全部元素都需要求解，通过一些假设可以简化为以下具体形式：
a B = [ 1 α y z α z y 0 1 α z x 0 0 1 ] [ s x a 0 0 0 s y a 0 0 0 s z a ] ( a S + [ b x a b y a b z a ] + v a ) a^B=\begin{bmatrix} 1 & \alpha_{yz} & \alpha_{zy} \\ 0 & 1 & \alpha_{zx} \\ 0 & 0 & 1\end{bmatrix}\begin{bmatrix} s_x^a & 0 & 0 \\ 0 & s_y^a & 0 \\ 0 & 0 & s_z^a\end{bmatrix}(a^S+\begin{bmatrix} b_x^a \\ b_y^a \\ b_z^a\end{bmatrix}+v^a) aB=⎣⎡100αyz10αzyαzx1⎦⎤⎣⎡sxa000sya000sza⎦⎤(aS+⎣⎡bxabyabza⎦⎤+va)
w B = [ 1 − γ y z γ z y γ x z 1 − γ z x − γ x y γ y x 1 ] [ s x g 0 0 0 s y g 0 0 0 s z g ] ( w S + [ b x g b y g b z g ] + v g ) w^B=\begin{bmatrix} 1 & -\gamma_{yz} & \gamma_{zy} \\ \gamma_{xz}& 1 & -\gamma_{zx} \\ -\gamma_{xy} & \gamma_{yx} & 1\end{bmatrix}\begin{bmatrix} s_x^g & 0 & 0 \\ 0 & s_y^g & 0 \\ 0 & 0 & s_z^g\end{bmatrix}(w^S+\begin{bmatrix} b_x^g \\ b_y^g \\ b_z^g\end{bmatrix}+v^g) wB=⎣⎡1γxz−γxy−γyz1γyxγzy−γzx1⎦⎤⎣⎡sxg000syg000szg⎦⎤(wS+⎣⎡bxgbygbzg⎦⎤+vg)
对于加计校准，未知参数有9个。如果IMU只有重力加速度的画，那么任意姿态下加计的读数带入公式（9）中得到的模长为g。因此构造公式（10）所示的优化目标函数，即求取使 L ( θ a c c ) L(\theta^{acc}) L(θacc)最小时的解，即为最优的未知参数。求解公式（10）可以借助Matlab的LM算法。要优化出各个未知参数，需要准备各姿态下IMU静止时的加计数据。
校准加计是将其读数和重力加速度比较。而要校准陀螺仪则是要和一段运动前后的标准姿态进行比较，而前后的标准姿态是根据重力加速度计算出的姿态。公式（13）就是这个依据这个原理的。当IMU静止时，通过加计读数可以解算出姿态，作为运动前的标准姿态。IMU经过一段时间旋转后再保持静止。我们可以有两种方式得到运动后的姿态，一是以运动前的标准姿态为初始条件使用角速度四元素积分可以得到运动后的姿态，二是静止后感知重力得到姿态，视作运动后再次静止的标准姿态。那么四元素解算和标准姿态可以进行一次对比。可以这样来很多次，那么就有足够多的数据导入到公式（13）中来优化出陀螺仪的未知参数（12）。注意：公式（12）中没有陀螺仪偏移量，陀螺仪偏移量和好测，取静止一段时间的读数平均值可以视作偏移量。
分析了校准原理，论文中给出了采集IMU数据的流程：1.先水平静止40-50s；2.转动到任意姿态，保持静止3-5s；3.再转动到任意姿态，保持静止3-5s；4.重复执行3，静止时的姿态保持均匀分布任意姿态，采取30个姿态的数据，理论上越多，校准越有效。