前言：继上篇6轴IMU之后，本文将介绍电子罗盘的原理（数学模型）和以AK09915为目标的应用实例。
磁基础知识磁的分类
硬磁：也就是永磁性材料。因为这种材料的矫顽力高/磁滞回线面基大，所以在被磁化后将很难退磁。
软磁：普通的纯铁就是很好的例子，可以被磁铁吸住（纯铁被磁化），但是把磁铁拿掉后，很快将不表现出磁性（退磁）。这种材料的矫顽力小/磁滞回线面基小，所以被磁化后将快速退磁；这种材料一般使用的目的就是增加磁导率，比如做成电感的磁芯可以加大电感量（$L=\mu\cdot S_{磁芯} \cdot N^2/l_{磁路}$）而又不带来大损耗。
地磁：地球产生的磁场，可以理解地球为永磁体，这里我们并不关注地球的磁性从何而来，只关心地磁场的模型（如下图）。NOTE：在地表测量地磁的磁感应强度范围约是0.4~0.6高斯，地磁场的大磁极轴与地球自转轴有一个约11.5°夹角。
磁物理量首先是常见的磁感应强度$B$，磁场强度$H$，表征磁介质磁性能力的磁导率$\mu=B/H$。磁感应强度$B$的单位有国际标准单位“特斯拉”(T)，也有常听到的“高斯”(G)，这两个量的关系为：$1T=10^4G$。
可以认为磁场强度是与激励源直接挂钩的量，比如电流$I$与磁强$H$的关系为：
$$H = \frac {NI}{l_{磁路}}$$
采用霍尔效应测量磁场（转换为霍尔电压$U$）时，属于直接测量磁感应强度$B$物理量：
$$U=\frac {I}{neh} B = KB$$
显然，霍尔器件理论上是具有良好线性测量特性，测量灵敏度为$K$。
地磁解算偏航角在开始前，我们先声名一下，坐标系设计为如下图所示的“北-东-地”坐标系，这是因为我们要测量的磁感应强度$\vec B$显然是一个从南往北的向量。
在校准好磁力计后，可以合理的假设我们测量得到的$\vec B$是较干净的地磁向量，则如下图所示分解为水平分向量$\vec B_{north}$和$\vec B_{down}$，此时我们的机头朝向显然为这里的X轴方向即$\vec x$，所以要得到偏航角可以用向量点乘的方法求解：
$$\alpha = \arcsin(\frac{\vec B_{north} \cdot \vec x}{\vert \vec B_{north} \vert \cdot \vert \vec x \vert})$$
所以接下来才是本文的重点——校准磁力计。
电子罗盘校准首先，建立带干扰的误差模型，上面提到过磁介材料有永磁材料、软磁材料，其中永磁材料相当于给测量结果加上一个偏置$H_1$，软磁材料相当于改变了测量结果与实际量的灵敏度（$S_m$）；除此之外，还有机械安装时的误差矩阵$S_c$（相当于旋转矩阵），制造时的刻度系数（灵敏度）误差矩阵$S_i$，轴间非正交误差矩阵$N$，零点偏移矩阵（$H_0$）。因此将测量结果$\hat B$与实际磁场$H$关系描述为：
$$\hat B = S_m S_c N S_i H+H_0+H_1$$
方便进行数学处理，我们可以合理地假定上式中的各个影响量是静态的，因此可以将上式各项合并简化为：
$$\hat B = K^{-1}H+P$$
所以我们的目标就是获得上式中的矩阵$K\in \mathbb R^{3\times3}$和偏置向量$P\in \mathbb R^3$，就可以校准测量向量$\hat B$为真实地磁向量$H$：
$$H = K(\hat B-P)$$
接下来我们讨论的是椭圆校准法，之所以叫这个是因为测量的数据点云呈现空间椭圆分布并且最后我们拟合的目标函数也是空间椭圆函数。因此，首先要说说为什么要用到椭圆这个形式，观察上式，$\hat B=[b_1,b_2,b_3]^T$的数据展示时相当于$[x,y,z]^T$坐标（实际看起来确实像椭圆），向量$P$就像是这堆数据点的中心（椭圆心），K就类似于这堆数据的空间范围（椭圆半径）。
考虑到地磁场在某一个地点的磁向量是固定的，不管磁力计再怎么旋转，测量得到的结果$\hat B$应该是表征出同样的地磁向量$H$。因为旋转不改变向量的模长，所以有：
$$\vert H \vert^2 \equiv | H_{\alpha,\beta,\theta}\vert^2 =\big( K(\hat B - P) \big)^{T}\cdot K(\hat B-P)\\=(\hat B^T-P^T)K^TK(\hat B-P)\\= \hat B^T K^TK \hat B - \hat B^T K^TK P - P^T K^TK \hat B+ P^T K^TKP$$
因为上式中，最后的形式里每一项都有$K^TK$，所以我们把这项用一个新的矩阵$A=K^TK\in R^{3\times 3}$替代，向量长因为是恒定的，我们用常量$s=\vert H \vert ^2$替代：
$$s=\hat B^T A \hat B - 2\hat B^T A P+ P^T AP,\\A = \begin{bmatrix}a_{11},a_{12},a_{13} \\a_{12},a_{22},a_{23} \\a_{13},a_{23},a_{33}\end{bmatrix}$$
注意：上式中的$- 2\hat B^T A P$本来是$-\hat B^T A P-P^TA\hat B$，但因为$A$是二次型矩阵所以两个结果是一样的就合并了。
把测量值用空间坐标表示$\hat B = [x,y,z]^T$，带入上式拆开后（有兴趣你可以试一试）：
$$ax^2+by^2+cz^2+dxy+exz+fyz+gx+hy+iz+l=s\\a=a_{11},b=a_{22},c=a_{33},d=a_{12}+a_{21},\cdots,l=P^TAP$$
得到拟合形式后，我们就可以用最小二乘法等数学工具将采样数据点拿去推参数了。但是上式中的实数项要合并一下还有参数项要拆出一个系数2方便后面的关系转换：
$$ax^2+by^2+cz^2+d2xy+e2xz+f2yz+g2x+h2y+i2z=1$$
将上式中要拟合的系数和样本数据分别用向量$V\in \mathbb R^{9\times 1}$和非方矩阵$D\in\mathbb R^{N\times 9}$来表示：
$$DV=1\\D=\begin{bmatrix}x_1^2 & y_1^2 & z_1^2 & 2x_1y_1 & 2x_1z_1 & 2y_1z_1 & 2x_1 & 2y_1 & 2z_1\\&&&& \vdots\\x_N^2 & y_N^2 & z_N^2 & 2x_Ny_N & 2x_Nz_N & 2y_Nz_N & 2x_N & 2y_N & 2z_N\end{bmatrix}\\V=\begin{bmatrix}a & b & c & d & e & f & g & h & i\end{bmatrix}^T$$
接下来使用非方矩阵的伪逆来求解$V$的最小二乘拟合（见文末）：
$$D^TDV=D^T\cdot 1\\V=(D^TD)^{-1}D^T \cdot 1$$
上面的“椭球拟合”图片，就是笔者使用AK09915的实际采样数据，用“非方矩阵伪逆法”拟合的参数效果。Matlab画图相关接口：scatter3、fimplicit3
那么拟合得到的系数，如何对应到校准方程中的矩阵系数呢？首先是偏移量向量：
$$O=A_{3\times3}^{-1} V_{ghi}\\A_{3\times3}=\begin{bmatrix}a & d & e \\d & b & f \\e & f & c\end{bmatrix} ,V_{ghi}=\begin{bmatrix}g \\ h \\ i\end{bmatrix}$$
接着求椭圆旋转矩阵，先求一个中间辅助矩阵：
$$B_{4\times4}=T A_{4\times 4}T^T\\T = \begin{bmatrix}1 &0 &0 &0 \\0 &1 &0 &0 \\0 &0 &1 &0 \\Ox & Oy & Oz & 1\end{bmatrix},A_{4\times 4}=\begin{bmatrix}a & d & e & g \\d & b & f & h \\e & f & c & i \\g & h & i & -1\end{bmatrix}$$
然后把这个辅助矩阵$B_{4\times4}$进行分块来求解另一个辅助矩阵：
$$B_{4\times4} = \begin{bmatrix}B_{3\times3-core} & N_{3\times 1}\\M_{1\times3} & b_{44}\end{bmatrix}\\B_{3\times3}=\frac{1}{-b_{44}} B_{3\times3-core}$$
对$B_{3\times3}$求解出3个特征值$\lambda_i$和3个特征向量$v_i$。则可以用特征值构造轴增益（椭圆半径）：
$$G=[\frac{1}{\sqrt{\lambda_1}},\frac{1}{\sqrt{\lambda_2}},\frac{1}{\sqrt{\lambda_3}}]^T$$
用特征向量构造旋转矩阵：
$$R_{3\times3}=\begin{bmatrix}v_1 & v_2 & v_3\end{bmatrix}$$
最终，我们就可以对读取到的传感器采样数据$\hat B$进行校准了：
$$\hat B_{withoutBias} = \hat B + O\\\hat B_{invRot} = R^{-1} \hat B_{withoutBias}\\\hat B_{pred} = \hat B_{invRot} / G$$
上面三步依次为：去偏移量、反旋转、去增益。
电子罗盘使用实战这一节主要有两个部分，分别是：
AK09915底层驱动程序：分享了从驱动一颗电子罗盘芯片到获得采样数据的全流程代码；
椭球矫正算法：接着在该部分中，分享了对采样数据进行分析得出干扰补偿数据的算法实现，并给出了矫正效果对比图；
矫正电子罗盘并求解航向角：最后这个部分，将矫正算法用C实现部署到了实际嵌入式硬件上，并求解最终目标——航向角。
AK09915底层驱动程序
上图是这个电子罗盘的寄存器表，可以看出这个器件可以配置的寄存器起始就三个：CNTL1、CNTL2、CNTL3。配置完成后，读取HXL~HZH的采样数据寄存器就可以了。初始化代码如下：
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960/** * @brief 设置工作模式 * @note 无 * @param targetMode: 要进入的工作模式 * @retval None */void AK09915_setOpMode(AK09915_operationMode_t targetMode){ /*保护其他位（FIFO，SDR）不被修改*/ uint8_t preStatus; AK09915_IIC_ReadRegs(MAGREG_CONTROL2, &preStatus, 1); preStatus &= 0xE0; // 1110 0000，清除MODE{4,0}位，变成PowerDown模式 // 进入Power Down 模式 AK09915_IIC_WriteRegs(MAGREG_CONTROL2, preStatus); // 等待1ms HAL_Delay(1); // 进入目标模式 if(targetMode != AK09915_operationMode_PowerDown) { preStatus |= targetMode; AK09915_IIC_WriteRegs(MAGREG_CONTROL2, preStatus); } return;}/** * @brief 设备初始化 * @note 使用前需初始化好IIC接口 * @param None * @retval None */void AK09915_initDev(void){ /*** * 配置 CNTL1 和 CNTL2 寄存器 * 使能噪声抑制滤波器，关闭FIFO，低噪声模式，200Hz采样率 ****/ uint8_t ctrlRegs[2] = { AK09915_NSF_EN, AK09915_FIFO_DIS | AK09915_SDR_LowNoise | AK09915_operationMode_ContinueMode1_10Hz }; // 设备复位 AK09915_resetDev_HardMethod(); // 进入空闲模式 AK09915_setOpMode(AK09915_operationMode_PowerDown); // 配置参数 AK09915_IIC_WriteRegs(MAGREG_CONTROL1,ctrlRegs[0]); AK09915_IIC_WriteRegs(MAGREG_CONTROL2,ctrlRegs[1]); // 读出参数，校验一下// HAL_Delay(1);// AK09915_IIC_ReadRegs(MAGREG_CONTROL1, ctrlRegs,2);}
上面的代码，把AK09915配置成了DRDY引脚输出有效数据，因此MCU可以使用外部GPIO中断来读数据：
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748/** * @brief 中断解析回调函数 * @note 在MCU中断引脚对应的EXTI函数中调用 * 开启测量后自动生成中断 * @param None * @retval None */void AK09915_callback_interrupt(void){ uint8_t statusCache; // - 读状态寄存器1 AK09915_IIC_ReadRegs(MAGREG_STATUS1, &statusCache, 1); // - 检验是否有数据，NOTE：这里优化去掉，因为是中断模式// if (statusCache & MAGREGMASK_CTRL1_DRDY)// {// /*有数据，读数据*/// } // - 检验是否有数据遗漏 if(statusCache & MAGREGMASK_CTRL1_DOR) { /*处理数据遗漏问题*/ } // - 读取数据 AK09915_IIC_ContinueReadRegs((uint8_t *)&ak09915_rawRegDataCache, sizeof(ak09915_rawRegDataCache)); // - 读状态寄存器2 AK09915_IIC_ContinueReadRegs(&statusCache, 1); // 一个Dummy AK09915_IIC_ContinueReadRegs(&statusCache, 1); // - 判断数据是否溢出（如果总磁强大于4912uT，芯片判为溢出） if(statusCache & MAGREGMASK_CTRL2_HOFL) { /*处理磁强爆表问题*/ } // - 判断数据有效与否（仅用于FIFO模式，标志FIFO缓冲区过读） #ifdef _USER_USE_AK09915_FIFO_ if(statusCache & MAGREGMASK_CTRL2_INV) { /*处理FIFO读到无效数据*/ } #endif}
最后给出上面用到的底层通信接口，读者可以根据需要优化成DMA和操作系统信号量的形式：
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144/******************************************************************** 通信封装*******************************************************************//** * @brief 向指定寄存器写入数据 * @note 一次只写一个数据！ * @param regAddr: 目标寄存器起始地址 * @param Data: 写入的数据 * @retval None */void AK09915_IIC_WriteRegs(uint8_t regAddr, uint8_t Data){ uint8_t ak09915_transCache[2]; ak09915_transCache[0] = regAddr; // 寄存器地址 ak09915_transCache[1] = Data; HAL_I2C_Master_Transmit(&AK09915DRV_IIC_HANDLER, /*HAL IIC Handler*/ AK09915DRV_IIC_ADDR, /*AK09915的IIC地址*/ ak09915_transCache, 2, /*传输的数据和长度*/ 0xff); /*超时阈值*/}/** * @brief 从指定寄存器读取数据 * @note 支持连续读一串数据，地址自增 * @param regAddr: 目标寄存器起始地址 * @param pData: 读取数据存放位置 * @param len : 读数据个数 * @retval None */void AK09915_IIC_ReadRegs(uint8_t regAddr, uint8_t * pData, uint8_t len){ uint8_t ak09915_transCache; ak09915_transCache = regAddr; // 寄存器地址 HAL_I2C_Master_Transmit(&AK09915DRV_IIC_HANDLER, /*HAL IIC Handler*/ AK09915DRV_IIC_ADDR, /*AK09915的IIC地址*/ &ak09915_transCache, 1, /*传输的数据和长度*/ 0xff); /*超时阈值*/ /*- 收数据 -*/ HAL_I2C_Master_Receive(&AK09915DRV_IIC_HANDLER, /*HAL IIC Handler*/ AK09915DRV_IIC_ADDR, /*AK09915的IIC地址*/ pData, len, /*传输的数据和长度*/ 0xff); /*超时阈值*/}/** * @brief 从上一次读取后继续读取 * @note 小心越界循环读取！ * @param pData: 读取数据存放位置 * @param len : 读数据个数 * @retval None */void AK09915_IIC_ContinueReadRegs(uint8_t * pData, uint8_t len){ /*- 收数据 -*/ HAL_I2C_Master_Receive(&AK09915DRV_IIC_HANDLER, /*HAL IIC Handler*/ AK09915DRV_IIC_ADDR, /*AK09915的IIC地址*/ pData, len, /*传输的数据和长度*/ 0xff); /*超时阈值*/}/******************************************************************** 基础功能*******************************************************************//** * @brief 硬件复位AK09915设备 * @note 提前配置RSTN引脚 * @param None * @retval None */void AK09915_resetDev_HardMethod(void){ // 拉低RSTN引脚 HAL_GPIO_WritePin(MAG_RSTN_GPIO_Port, MAG_RSTN_Pin, GPIO_PIN_RESET); // 等待1ms HAL_Delay(1); // 拉高RSTN引脚 HAL_GPIO_WritePin(MAG_RSTN_GPIO_Port, MAG_RSTN_Pin, GPIO_PIN_SET); }/** * @brief 读取设备号通信测试 * @note 检验 WHOIAM 寄存器数据 * @param None * @retval 0正确；1通信错误；2数值错误 */uint32_t AK09915_communicateTest(void){ uint8_t ak09915_testCache[2]; AK09915_resetDev_HardMethod(); // 复位并使能设备 // 发地址 ak09915_testCache[0] = MAGREG_WHOIAM1_COMPANYID; if(HAL_I2C_Master_Transmit(&AK09915DRV_IIC_HANDLER, AK09915DRV_IIC_ADDR, ak09915_testCache, 1, 0xff) \ != HAL_OK) { return 1; // 通信错误 } // 读取WHOAMI1和WHOAMI2数据 if (HAL_I2C_Master_Receive(&AK09915DRV_IIC_HANDLER, AK09915DRV_IIC_ADDR, ak09915_testCache, 2, 0xff) \ != HAL_OK) { return 1; // 通信错误 } // 检验正确性 if(ak09915_testCache[0] == MAGREGVAL_WHOIAM1_COMPANYID \ && \ ak09915_testCache[1] == MAGREGVAL_WHOIAM2_DEVICEID) { return 0; // 正确 } else return 2; // 数值错误}/** * @brief 获取地磁数据 * @note 原始16位有符号数据 * @param pRes : 存放读取数据的位置 * @retval None */void AK09915_getRawData(AK09915_rawRegData_t * pRes){ AK09915_IIC_ReadRegs(MAGREG_X_L, (uint8_t *)pRes, sizeof(AK09915_rawRegData_t));}
必要类型与变量的声名：
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118#ifndef __AK09915_DEFS_H__#define __AK09915_DEFS_H__#include "stdint.h"/******************************************************************** 数据格式*******************************************************************/typedef struct{ union{ uint8_t L_H_bytes[2]; int16_t getSigned; } HX; union{ uint8_t L_H_bytes[2]; int16_t getSigned; } HY; union{ uint8_t L_H_bytes[2]; int16_t getSigned; } HZ; } AK09915_rawRegData_t;/******************************************************************** 寄存器地址表*******************************************************************/#define MAGREG_WHOIAM1_COMPANYID 0x00 // AK公司ID#define MAGREG_WHOIAM2_DEVICEID 0x01 // 设备ID#define MAGREG_STATUS1 0x10 // 状态：数据准备、数据覆盖#define MAGREG_X_L 0x11#define MAGREG_X_H 0x12#define MAGREG_Y_L 0x13#define MAGREG_Y_H 0x14#define MAGREG_Z_L 0x15#define MAGREG_Z_H 0x16#define MAGREG_STATUS2 0x18 // 状态：磁量溢出、无效数据#define MAGREG_CONTROL1 0x30 // 配置：NSF使能、FIFO的阈值#define MAGREG_CONTROL2 0x31 // 配置：工作模式、功耗模式、FIFO使能#define MAGREG_CONTROL3 0x32 // 配置：软件复位/******************************************************************** 寄存器内容*******************************************************************/#define MAGREGVAL_WHOIAM1_COMPANYID (0x48)#define MAGREGVAL_WHOIAM2_DEVICEID (0x10)/** * @reg MAGREG_STATUS1***/#define MAGREGMASK_CTRL1_DRDY 0x01 // 数据准备好了#define MAGREGMASK_CTRL1_DOR 0x02 // 数据覆盖#define MAGREGMASK_CTRL1_HSM 0x80 // 高速IIC 模式/** * @reg MAGREG_STATUS2***/#define MAGREGMASK_CTRL2_INV 0x04 // 无效数据#define MAGREGMASK_CTRL2_HOFL 0x08 // 数据溢出/** * @reg MAGREG_CONTROL1***/typedef enum{ AK09915_NSF_EN = (0x01<<5), AK09915_NSF_DIS = (0x00<<5),} AK09915_NSF_t;/** * @reg MAGREG_CONTROL2***/typedef enum{ AK09915_operationMode_PowerDown = 0x0, AK09915_operationMode_SingleMeasure = 0x1, AK09915_operationMode_ContinueMode1_10Hz = 0x2, AK09915_operationMode_ContinueMode2_20Hz = 0x4, AK09915_operationMode_ContinueMode3_50Hz = 0x6, AK09915_operationMode_ContinueMode4_100Hz= 0x8, AK09915_operationMode_ContinueMode5_200Hz= 0xA, AK09915_operationMode_ContinueMode6_1Hz = 0xC, AK09915_operationMode_SelfTest = 0x10,} AK09915_operationMode_t;typedef enum{ AK09915_SDR_LowPower = (0x0<<6), AK09915_SDR_LowNoise = (0x1<<6),} AK09915_SDR_t;typedef enum{ AK09915_FIFO_EN = (0x01<<7), AK09915_FIFO_DIS = (0x00<<7),} AK09915_FIFO_t;/** * @reg MAGREG_CONTROL3***/typedef enum{ AK09915_SRST_EN = 1, AK09915_SRST_DIS = 0,} AK09915_SRST_t;#endif
椭球矫正算法
为了方便算法的验证，这里使用Matlab作为主要编程工具，上图就是对原始采样数据矫正前后的效果对比（显然矫正前后椭球的中心补偿到了零点，椭球的轴长比接近1：1也就是圆球）。当然也可以使用Python或者纯C实现（最后部署时还得是C），但是Py和C在矫正效果展示方面（3D绘图）实在太困难了，没有Matlab方便。以下代码即可在Matlab上直接实现原始数据的分析、矫正、数据展示：
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112%% 配置工作条件 ----------------------------------------------------------% 存储电子罗盘采样数据的文件名compassDataFileName = "ak09915.csv";% 显示图像的方式：1. 子图形式， 2. 分别显示SHOW_FIG_PATTERN_METHOD = 1;%% 读取采样数据 -----------------------------------------------------------% 自动解析CSV文件，自动识别并去掉表头，返回矩阵类型elecCompassDataTable = readmatrix(compassDataFileName);% 提取3个轴向数据向量xVector = elecCompassDataTable(:,1);yVector = elecCompassDataTable(:,2);zVector = elecCompassDataTable(:,3);%% 求解标准椭球方程的9个参数 -----------------------------------------------epis9Param = fitEpisode9Param(xVector,yVector,zVector);%% 显示原始数据与拟合效果 --------------------------------------------------figureif SHOW_FIG_PATTERN_METHOD == 1 subplot(1,2,1)end% 计算3D显示的数据范围intv = [min(xVector) max(xVector) min(yVector) max(yVector) min(zVector) max(zVector)];intv = 1.2 * intv; % 比顶端数据范围大20%% 标准9参数椭球方程f = @(x,y,z) epis9Param(1)*x.^2 + epis9Param(2)* y.^2 + epis9Param(3)* z.^2 + ... epis9Param(4)*2*x*y + epis9Param(5)*2*x*z + epis9Param(6)*2*y*z + ... epis9Param(7)*2*x + epis9Param(8)*2*y + epis9Param(9)*2*z -1;% 绘制椭球面fimplicit3(f,intv)% 添加原始采样数据散点hold onscatter3(xVector,yVector,zVector)% 添加标题和轴标title("电子罗盘采样数据-非方矩阵的伪逆法拟合效果")xlabel("x")ylabel("y")zlabel("z")%% 求解校准矩阵------------------------------------------------------------% 中心偏移向量A3x3 = [ epis9Param(1) epis9Param(4) epis9Param(5) ; epis9Param(4) epis9Param(2) epis9Param(6) ; epis9Param(5) epis9Param(6) epis9Param(3) ];Vghi = [ epis9Param(7); epis9Param(8); epis9Param(9) ];CentorOffVec = inv(A3x3)*Vghi;% 旋转矩阵T = [ 1 0 0 0 0 1 0 0 0 0 1 0 CentorOffVec(1) CentorOffVec(2) CentorOffVec(3) 1];A4x4 = [A3x3 Vghi Vghi' -1 ];B4x4 = T * A4x4 * T' ;B3x3 = B4x4(1:3,1:3) / (-B4x4(4,4));[eigVector,eigLambda] = eig(B3x3); % 分解特征向量和特征值invRot = inv(eigVector); % 反旋转矩阵invGainVector = [eigLambda(1,1); eigLambda(2,2); eigLambda(3,3)];invGainVector = 1 ./ sqrt(invGainVector); % 各轴增益补偿矩阵%% 对原本采样的数据进行矫正-------------------------------------------------xyzRectData = [xVector, yVector, zVector]; % 3轴数据组合起来更快xyzRectData = xyzRectData + CentorOffVec'; % 球心偏置矫正xyzRectData = invRot * xyzRectData';xyzRectData = xyzRectData'; % 旋转矫正xyzRectData = xyzRectData ./ invGainVector';%% 显示矫正后的数据---------------------------------------------------------if SHOW_FIG_PATTERN_METHOD == 1 subplot(1,2,2);else if SHOW_FIG_PATTERN_METHOD == 2 figure; endendxRectVector = xyzRectData(:,1);yRectVector = xyzRectData(:,2);zRectVector = xyzRectData(:,3);% 再按照9参数椭球标准拟合一次episRect9Param = fitEpisode9Param(xRectVector,yRectVector,zRectVector);% 计算3D显示的数据范围intv = [min(xRectVector) max(xRectVector) min(yRectVector) max(yRectVector) min(zRectVector) max(zRectVector)];intv = 1.2 * intv; % 比顶端数据范围大20%% 标准9参数椭球方程rectF = @(x,y,z) episRect9Param(1)*x.^2 + episRect9Param(2)* y.^2 + episRect9Param(3)* z.^2 + ... episRect9Param(4)*2*x*y + episRect9Param(5)*2*x*z + episRect9Param(6)*2*y*z + ... episRect9Param(7)*2*x + episRect9Param(8)*2*y + episRect9Param(9)*2*z -1;% 绘制椭球面fimplicit3(rectF,intv)% 添加原始采样数据散点hold onscatter3(xRectVector,yRectVector,zRectVector)% 添加标题和轴标title("电子罗盘矫正数据")xlabel("x")ylabel("y")zlabel("z")
其中，对椭球表面的采样散点数据进行最小二乘拟合参数的函数fitEpisode9Param实现很简单，就是上面的非方矩阵的伪逆公式的代码形式：
1234567function [fitParamVector] = fitEpisode9Param(xVector,yVector,zVector) D = [ xVector.*xVector, yVector.*yVector, zVector.*zVector, ... 2*xVector.*yVector, 2*xVector.*zVector, 2*yVector.*zVector, ... 2*xVector, 2*yVector, 2*zVector]; fitParamVector = inv(D' * D) * D' * ones(length(xVector),1);end
矫正电子罗盘并求解航向角（嵌入式）最后一步了，就是把矫正部署到实际硬件上，并且根据得到的地磁向量直接求解出航向角。由于上面已经多次讨论并且展示了矫正算法，所以我们可以很容易地用C语言实现如下：
123456789101112131415161718192021222324252627282930313233343536373839404142434445#include "math.h"/** * @brief 椭球矫正算法 * @note 输入参数是标准9参数转换后的矩阵形式 * @param pSrc : 磁传感数据存放的位置 * @retval None */void episodeRectify(mag_rawData_t * pSrc){ float EpiCmptCache[3]; // 中心偏移 B_{noBias} = B_{samp}-Offset 注意这里补偿的正负号 pSrc->compass[0] = pSrc->magX + pSrc->centerOffset[0]; pSrc->compass[1] = pSrc->magY + pSrc->centerOffset[1]; pSrc->compass[2] = pSrc->magZ + pSrc->centerOffset[2]; // 反旋转 B_{noRot} = invRot * B_{noBias} for (int i =0; i<3; i++) // 逐行求 { EpiCmptCache[i] = pSrc->invRot[i][0] * pSrc->compass[0] + \ pSrc->invRot[i][1] * pSrc->compass[1] + \ pSrc->invRot[i][2] * pSrc->compass[2]; // 矩阵乘法 } // 轴长补偿 注意：这里使用乘法（因为比除法更快），所以加载补偿量时应该是 1/轴长 for (int i =0; i<3; i++) { pSrc->compass[i] = EpiCmptCache[i] * pSrc->gain[i]; }}/** * @brief 解算航向角 * @note 若严重非水平，应当传入旋转补偿后的磁向量 * @param compass3Axis : x,y,z轴的磁向量数据 * @retval yaw 偏航角 */float getYaw_FromCompass(float compass3Axis[3]){ /** Y */ /* 水平情况下偏航角求解公式 ： arctan( ------) */ /* X */ return atan2( compass3Axis[1] , compass3Axis[0] ); // 参见 math.h}
最后简单点一下上面实现中的一些小技巧。首先，矫正算法中的旋转补偿这里在加载参数的时候就以$R^{-1}$的形式，这样可以节省非常多的计算量；接着，在各轴向补偿上，原本是$/G$，但是在硬件上浮点除法是比乘法要难算的，所以这里我们在加载参数的时候就以$1/G$的形式加载，将除法换成了乘法；最后，求航向角使用的是ARM的数学库，推荐读者在使用不同处理器时进行相应的优化（如果有的话）。
参考链接
意法半导体公司对用于传感器校准的椭球或球体拟合的设计建议
矩阵最小二乘-知乎作者：你是想难为我胖虎
Donate