龙猫的博客

以上的代价函数是有加法的，运用最小二乘法来估计参数比较困难，因此使用 LM 算法来进行非线性估计，具体 LM 算法的原理与实现，参考主页中的上一篇文章《LM迭代估计法》，其与标准正交坐标系有一定的偏移，将各轴的偏移记作 β，之后建立误差模型，建立代价函数，进而求解估计参数。其中，T 为旋转矩阵，W(O) 表示陀螺仪的正交标准轴，与 AOF 重合，W(S) 是当前的陀螺仪轴。此方程中，含有 9 个未知量，三轴的轴偏移，三轴的比例系数，三轴的零偏。，理想的三轴坐标系，加速度计的理想坐标系可以记作 AOF，

磁力计的数据在实际中是椭球的形状，在此之前使用了球体拟合进行校准，也就是简化为正球体的模型，得出的结果比较差，航向计算不准，还是需要用椭球的模型来估计偏移量，先使用标准的椭球方程，进行化简与变形，得到最小二乘法可以进行估计的标准形式，之后对原始数据进行最小二乘法矩阵的赋值，求解方程，最终观察拟合效果。但使用此方程会有问题，可以求出多个解，使用matlab求解的话，很容易就会求出异常的解，也就是Rx，Ry，Rz很大的时候，也是会满足方程的，但这个解肯定是不对的。实际输出为：y = φT * θ。