OK，接下来是重头戏。
我们按照文章2中说的方法对加速度进行校准。
先通过最小二乘法校准使得测得向量模长与真实向量模长相等，再通过姿态矩阵把它和真实向量对齐。
也就是$a_c = C_a(K_aa_r+B_a)$（2）
这个校准方法比那个论文里的，稍微复杂点，加了个
$a_c$校准后加速度数据
$a_r$校准前加速度数据
$K$、$B$校准矩阵，前者3×3，后者3×1，共12个待定量，自由度12
这里我们使用椭球校准。显然，如果没有误差，那么数据点会分布在一个球上，我们这样构造的误差函数：
$a_m = K_aa_r+B_a = (x_m,y_m,z_m)$（3）
$error(x,y,z) = x^2+y^2+z^2-r^2$（4）
（这里a的下标m就代表中间吧……）
这个误差函数的自变量是xyz，而参数是那12个待定量。这个r是我们给定的，比如我们给9.8或者1024（对原始数据）。
看上去很复杂，但是由于有最小二乘法的库函数，所以我们只需要调用库函数就行。
$C_a$姿态转移矩阵，这是一个正交矩阵，可以由三个量完全确定，可以是归一化四元数的三个，可以是方向余弦，这里选择使用理解是最简单的欧拉角。
注意，以下只是借助姿态转移矩阵来让两个向量对齐！这里的欧拉角没有实际物理意义！后面再次用到欧拉角的时候则有！后面会解释的。
欧拉角的定义有很多种，这里使用$-Z\rightarrow X\rightarrow Y$内旋。这只是方便，因为后面要解算姿态的时候也用的是这种，理论上那种都没问题，只需要保证能描述三维旋转，让实测的向量与真实的向量对齐就行。
解释一下这个定义：机体的固连坐标系初始对准XYZ东北天，先绕自己的z轴顺时针旋转偏航角Yaw（也用$\psi$），再绕自己的x轴逆时针旋转俯仰角Pitch（也用$\theta$），最后绕着自己的y轴逆时针旋转Row（也用$\gamma$）。注意，每次旋转自己的轴也会相应旋转！
由于欧拉角的定义有很多种，《惯性导航》和mpl库使用的是这种，所以我就用这种了。很多领域用到的欧拉角都是为了各个领域的方便都不一样，我之前写的那篇定点转动的文章种的欧拉角和这里都不一样！所以我另外放图。
图源《惯性导航》（秦永元）
接下来我们要看看怎么由欧拉角推导姿态转移矩阵了。
额这……实在有点过于复杂……我不想latex打了……上图吧，同书……
这里三角对应不一样，看着有点不方便……先将就下，后面会给程序……
那么这里我们这样构造误差函数：
$a_r = C_aa_m = (x_c,y_c,z_c)$（5）
$g = (g_x,g_y,g_z)$（6）
$error(x_c,y_c,z_c,g_x,g_y,g_z) = \sqrt{(x_c-g_x)^2+(y_c-g_y)^2+(z_c-g_z)^2}$（7）
这个误差函数自变量就是那6个，参数是三个欧拉角
我们后面都是用scipy.optimize中的least_squares函数计算。这个方法不是最快的，但肯定能跑，毕竟电脑上用来校准的程序不求什么运算速度。
下面分布给程序和操作，最后会给出总的程序，把前面的程序用作库。
声明：本文最后会使用ai辅助编写部分代码，我在写文到一半才想起ai，所以开始的时候文章没有标注有ai生成的部分，现在打算编辑，却发现编辑不了，所以在这里声明。所有ai生成的部分均不包括核心部分且均经过我的检验，可以信任。
那就先看第一步，椭球校准。
先上python程序，我就先不打包了，今天的这俩相当于库文件，明天搞好整体后再打包成exe
由于有numpy和scipy库，这个程序很简单，简要介绍下就行。
这里的就是用于计算的库，还加了俩绘制3d显示图的。
实现前面说的误差函数。
最小二乘法，least_squares的使用方法可以csdn查，这里简要说下。这里传入误差函数，初始猜测值（由于非线性最小二乘，计算涉及偏导求极值，所以这个值偏差太大的话可能得到错误的结果，但是这个传感器还是相对准的，我们猜测是$K = I,B = O$），args是误差函数里除了params这个numpy数组外其他的参数。这个max_nfev是最大拟合次数，默认1200，实测不够，所以设置大点，否则会报错，不过10000绝对够了。
获取结果，不必多说。为了方便直接返回两个矩阵。
绘图函数，不是重点，具体不说了，就是绘制原始数据，校准后数据和理想球面。
测试函数，用逆矩阵等办法，从球面数据模拟传感器实测数据。
不必多说。
总之，这作为一个库，不止现在校准加速度用，后面校准磁力计也会用。
点击运行。
这是输出的。
这里参数会有点偏差，但是是没问题的。这是数学原因，这里只保证了数据分布在球面上，至于向量对准就是下一步的事情了。
那么怎么看是否分布在球面上呢?
因为投影等原因，那个球面看上去有点变形了，但它的确是球面。
可以看出校准后的数据远比原始数据贴近球面。
对了说下，这个图是可以用鼠标拖动旋转的
当然这里的数据肯定是非常理想的，相对均匀分布（当然，因为为了简便，这里直接用参数方程生成数据点，这导致数据是“两极”密集，“赤道”稀疏）。到时候校准传感器的数据为了保证静止，不可能这么均匀，但是磁力计是可以接近的。不过不管怎么说，只要多几个姿态，校准也是够用的。具体的操作还是搞定整个程序再说吧。
然后是姿态校准。
这里和刚刚的程序一样，引入库。
实现开始说的误差函数。可以看出这个姿态矩阵有多恶心……还要矩阵乘法，最小二乘法又要偏导，还有三角函数……不过不在单片机上算，而且别人也写好了库，那么开发速度肯定是要远重要于程序的时间复杂度的……如果用四元数啥的这里就让人脑麻烦了……
一样，最小二乘法。这里gx这几个变量不一定代指的是重力加速度哈，也可以是磁力计，我想不到啥不会混淆的字母了
。这里我们也是相信传感器比较准确。
一样，打印参数，返回矩阵。
绘图。毕竟是给人看的，检查运算结果，我也在想怎么最直观又能简便地实现，想来想去还是决定直接画误差向量对应的点。如果有别的好的想法麻烦再下面提出，谢谢了。
测试函数，思路一样，具体实现不用管。为了简便，这样用参数方程肯定会导致他只是对天顶角和方位角均匀，表现出数据点会出现“两极”密集“赤道”稀疏，但是毕竟只是实验算法，不用管。
注意的地方就是这里numpy库的三角函数都是以弧度制为输入的，测试的时候欧拉角不要调太大。（实际上传感器也不会偏差太多，最多十几度）
一样。
这里说下那个噪声，这里原始数据向量模长是1，上个程序用了4，所以这里噪声相应减小了。不过这不是重点。
点击运行。
这次没有数学上影响准确度的事情，可以看出非常准了。
我之前不加噪声发现原始数据应该是一个圆盘分布的，加了噪声就这样了。可以看出校准成效很好，没有噪声基本就是一个点。
不过这里有个问题，为啥原始数据是圆盘分布的？我还搞不清楚，我直觉是个球啊，如果有大佬能帮忙那感激不尽。
顺便说下，我发现我生成原始数据那里，球面用的不是天顶角，因为那个sin和cos搞反了，不过没有关系，
懒得改了。总之先到这里了，明天我要看看这个原始数据圆盘分布到底有没有问题，然后搞总程序。
同样的，这也是总程序的一个库
补豪！第二个程序有问题！怪不得原始误差不是球。开始还没发现！刚刚才觉得有点问题！不能这样用这个矩阵！他变的是坐标系……明天改下！涉及到第二个程序的先别看，我改完删掉这条回复才算正常！现在验证了并没有问题！（而且论坛好像不能删除回复）
直接用这个矩阵没问题的，因为整体误差就是差了一个姿态转移矩阵，类似二次型那里斜椭球转正椭球。我模拟生成了一些数据点，误差向量对应的点的确分布在圆盘上！
简要叙述下数学证明：
$a_c = Ca_m$
$\delta a = a_c - a_m = (I-C)a_m$
这个$I-C$的行列式是零，不满秩！(证略，使用四元数形式的姿态转移矩阵相对好证，但是还是计算繁琐，不是重点就略去了)
然后他当然会把分布再球面上的$a_m$压缩成一个圆盘！
总之，实验这种校准方式是完全没问题的！！！！！！现在开始加速度校准整体程序的开发，希望今天内搞定！！！！不过大概没有图形化窗口了，我觉得这个暂时没必要。
发现手动复制串口助手中的数据输入太麻烦了，我打算直接写个程序连接串口，顺便把图形化界面做了，所以可能要等一会。
还有，为了简便，姿态校准只使用一组数据即可。这影响不大。这是因为椭球校准后误差完全由一个三维旋转引起，我们这里也只关注初末状态，所以一组数据校准了，这个矩阵对其他数据也是有用的。
我还没有搞过控制
不过可以给几点建议，就是简单的话那个加速度计不用搞斜面啥的，静止就行，因为用斜面的话实测加速度的抖动比较大。另外控制算法可以试试用PID算法代替延时，应该是有用的，但是PID要实验调参，感觉如果没有什么模拟软件的话要多次发射才行
各位麻烦等一下
我写的程序在我舍友的电脑上工作一切正常，但是在我的电脑上甚至打不开串口……有时候打开了又读取不了……
它为什么不能跑……它为什么又能跑了……
更新：
此问题已经解决，在使用本程序的时候，请保证360，小鸟壁纸，鲁大师，金山毒霸，2345等处于关闭状态！
其次，我在第三篇那里更新了，添加了LED指示灯指示是否初始化成功，请在校准中检查
OK，搞定了！这个.py文件要放到前两个文件同路径下，然后点击运行就行，要求电脑装了python。当然，没有的话可以直接下载后面打包好的.exe文件，这个可以直接用。代码就不详细介绍了，因为这个主要是关于串口接收的。使用了TKinter做图形化界面，使用pySerial做串口相关。程序的大纲是ai生成的，我主要把收集数据和数据处理的部分写了。（ai在做这种图形化界面之类的麻烦但逻辑性不强的程序方面挺不错的）（补充一下由于版本原因这个代码的实际运行和下方的图片存在极其细微的差别，比如输出的文字之类，请忽略）
这是点进去的界面，同时会弹出黑框，为了方便看部分数据，以及为了写单片机代码的时候方便把数据给cv了，所以就没有去掉。
配置好后可以打开串口。（配置如图，当然串口号不一定一样），如果发现打不开，可以尝试点击单片机reset，或者把串口拔掉重插，并让传感器水平放置一会，让mpu9250通过初始化。也有可能是电脑的事情，重装最新版的ch340驱动，然后重启就行。（我昨天就是这样）
前六个数据按照提示要求摆姿态，先z向上向下，然后y，然后x，这三轴在传感器电路板上画了。后面的姿态就没有要求了，尽量和前面不同，均匀些。
点击确认开始读取数据，看上面的进度条。总共100个数据，最后会取均值处理。如果读不到数据，进度条不动的话，处理方法同上。
静止时，100个数据的标准差实测在10以内，如果检测到超过了会判断为未静止，然后弹出这个框，然后自动重测。它可能会弹出很多个，不过如果弹出太多可以尝试全部关掉（它弹出的速度不会很快），然后点击关闭串口，或者关掉程序重来。
这样就是一个姿态的数据读取完毕了。
这样是10个姿态全部读取完毕了，按提示点击关闭串口。
接下来会弹出椭球校准的图，比较慢，要等个几秒。检查校准后数据点是否均匀，是否贴近球面。
关掉后会弹出姿态校准的图，检查校准后误差点是否接近原点。
关掉后会弹出弹窗，有数据。
这里先不放图了，图放后面。
这是打印出的数据，最下面是结果。
过程中也有很多数据，有些是均值，有些是标准差，最下面的俩是运算次数和残差。不过一般不看。
校准完毕后，仍然可以再次打开串口，继续校准，接下来也是10个姿态一组，不过不要求前六个特定姿态。之前的数据仍然保存。如果你认为十个姿态不够或者之前数据点没有均匀分布，可以继续校准。
我们下午因为桦加沙停课了，下午会继续更新，用30个姿态校准后，写进单片机程序里。这个更完之后本篇就完了，接下来是下一篇的磁力计校准。
补三张校准实景图
我这里用了哪种焊接元件用的磁吸万向夹，很便宜，也足够稳。如果有条件的话，那种三脚架上的带阻尼的万向头最好。最好不要只是直接随便放置传感器，主要是mpu9250要连个相对硬的杜邦线，这导致它随便放的话有些姿态不太容易摆到。
上面那是块铁片，用来吸台灯的。
然后有几点注意的。一手指不要碰到mpu9250芯片的引脚，还有那几个电容，手指导电，会让数据出错。二摆前六个姿态一般用手按着，稍微用点力。三桌子不一定水平或竖直，要先拿水平仪量一下。四后面的姿态用万向夹的时候，注意单片机开发板不要物理意义上悬空，否则他通过杜邦线拉扯mpu9250，会不稳。
猜猜为啥我这么晚才发，下午台风停课和同学去玩狼人杀了。校准了2轮，20个数据点。
以下是椭球拟合结果，可以看到各数据点贴合球面。但似乎不够均匀，不过问题不大。
这是姿态拟合的结果，可以看出校准后误差很小
这是最终结果
$a_c = C_a(K_aa_r+B_a) = CK_aa_r+CB_a$（8）
（矩阵分配进去，乘积当一个矩阵了，下标随缘~后面以（8）为准）
这是到时候单片机里面要运行的校准代码。这两个矩阵在上图。
改了单片机代码试了下，发现只用一个点做姿态校准的话，仅在z轴竖直向上时精确，在其他姿态下，最多可以有5%的误差。所以这里我选择用6个姿态点进行姿态校准。然而，理论上不应该出现这个问题，这可能是一些未考虑的非线性误差的影响。
还有一个解决方法，就是姿态矩阵不规定用欧拉角表示，而是直接9个待定量。好处是能尽可能贴合这6个姿态，但是坏处是它的行列式可能偏离1，对其他姿态有影响，而且没有理论支持。原本结果误差不大，所以就不这么做了。
加了一些用于计算的函数。当然，有浮点数精度问题还有算法不是最速的，不过个人认为无伤大雅，因为这里主要误差和耗时不在这里。
这里是校准的部分。还有我上传的程序116行那个注释别管它
，我忘记删了，不过我不想为了一个注释重新上传……
另外之前python程序那里注释有错别字，不过……大家看得懂就行……
是通假字好了！终于，1.4更完了，明天开始1.5！
对了这里说件事情。由于之前我不熟悉numpy库，那个姿态校准的地方我原本想的是误差函数返回一个ndarray数组，然后这个数组里放每个测量重力向量与实际重力向量差向量大小，但是那个norm方法这么用会直接返回整个矩阵的范数，也就是说，误差函数直接返回了，那六个差向量大小的平方和再开根。但是，神奇的是，今天测试发现，它这么做精度反而更高。所以能用，好用，就不改了。
再补充一下，这个程序毕竟是适用于不同传感器的，如果分辨率不同，或者量程设置不同，最后acc_XXXXXXXXXXXXXX的这里（图中光标处）要改。
再补充一下，那个椭球校准的库的3d绘图绘制球面的地方，那个注释写的透明度错了，因为我后面改过。我不想重新上传了，这只是个注释，就将就看吧