基于最小二乘法的磁力计椭球拟合方法

在写飞控代码时，必然要对磁力计的测量数据进行校正，本文将介绍一种简单实用的校正方法–基于最小二乘法的椭球拟合方法。

本文椭球拟合部分来自博文IMU加速度、磁力计校正－－椭球拟合，本文对最小二乘估计进行了部分推导，最后使用实测的数据完成了磁力计的椭球拟合。

椭球拟合

磁力计在测量磁场强度时，在环境不变的情况下，传感器每个姿态感受磁场强度是相同的，磁力计测量的x,y,z轴值，在没有偏差且传感器内部x,y,z轴相互垂直的情况下，在三维空间中组成一个圆球面。但是磁力计存在Hard Iron Distortion和Soft Iron Distortion。使得x,y,z轴度量单位不相同，各轴也并非相互垂直，(说明一下，任意椭球的三个轴都是相互垂直的，几何上，椭球最长的轴与最短的轴相互垂直，从代数的角度看，对称正定矩阵A=R′BRA=R^{\prime} B RA=R′BR，其中BBB为对角线大于0表示各轴长度的对角矩阵，RRR为旋转矩阵，R′R=IR^{\prime} R=IR′R=I，所以磁通量的空间坐标虽然形成一个椭球，椭球各轴相互垂直，但这个垂直的轴已经不是传感器x,y,zx,y,zx,y,z轴了)椭球球心也并非[0,0,0]，坐标磁通量在三维空间组成的椭球球心，是磁力计的校准值的一部分。

数学模型:

我们让磁力计尽可能多地采集到空间各个方向上的磁场强度，最后的测量数据将会形成空间上的一个椭圆，而校正问题在于给定椭球球面上的点，如何求椭球球心。其实就是一个椭球拟合问题。

a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1a_{1} x^{2}+a_{2} y^{2}+a_{3} z^{2}+a_{4} x y+a_{5} x z+a_{6} y z+a_{7} x+a_{8}y+a_{9} z=1a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1

从几何的角度表示上式的椭球为：
[x−cxy−cyz−cz][r11r12r13r21r22r23r31r32r33]T[λ1000λ2000λ3][r11r12r13r21r22r23r31r32r33][x−cxy−cyz−cz]\left[ \begin{array}{ccc}{x-c_{x}} & {y-c_{y}} & {z-c_{z}}\end{array}\right] \left[ \begin{array}{ccc}{r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}}\end{array}\right]^{T} \left[ \begin{array}{ccc}{\lambda_{1}} & {0} & {0} \\ {0} & {\lambda_{2}} & {0} \\ {0} & {0} & {\lambda_{3}}\end{array}\right] \left[ \begin{array}{ccc}{r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}}\end{array}\right] \left[ \begin{array}{c}{x-c_{x}} \\ {y-c_{y}} \\ {z-c_{z}}\end{array}\right] [x−cxy−cyz−cz]⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤T⎣⎡λ1000λ2000λ3⎦⎤⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤⎣⎡x−cxy−cyz−cz⎦⎤
=1+[cxcycz][r11r12r13r21r22r23r31r32r33]T[λ1000λ2000λ3][r11r12r13r21r22r23r31r32r33][cxcycz]=1+\left[ \begin{array}{lll}{c_{x}} & {c_{y}} & {c_{z}}\end{array}\right] \left[ \begin{array}{ccc}{r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}}\end{array}\right]^{T} \left[ \begin{array}{ccc}{\lambda_{1}} & {0} & {0} \\ {0} & {\lambda_{2}} & {0} \\ {0} & {0} & {\lambda_{3}}\end{array}\right] \left[ \begin{array}{ccc}{r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}}\end{array}\right] \left[ \begin{array}{c}{c_{x}} \\ {c_{y}} \\ {c_{z}}\end{array}\right] =1+[cxcycz]⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤T⎣⎡λ1000λ2000λ3⎦⎤⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤⎣⎡cxcycz⎦⎤
写成矩阵形式：
[X−C]M[X−C]T=1+CMCT[X-C] M[X-C]^{T}=1+C M C^{T} [X−C]M[X−C]T=1+CMCT
XMXT−2CMXT+CMCT=1+CMCTX M X^{T}-2 C M X^{T}+C M C^{T}=1+C M C^{T} XMXT−2CMXT+CMCT=1+CMCT
其中X=[xyz]X=\left[ \begin{array}{lll}{x} & {y} & {z}\end{array}\right]X=[xyz]表示椭球上的点，C=[cxcycz]C=\left[ \begin{array}{lll}{c_{x}} & {c_{y}} & {c_{z}}\end{array}\right]C=[cxcycz]表示椭球的球心；

M=RTBR=[r11r12r13r21r22r23r31r32r33]T[λ1000λ2000λ3][r11r12r13r21r22r23r31r32r33]=[a1a4/2a5/2a4/2a2a6/2a5/2a6/2a3]M=R^{T} B R=\left[ \begin{array}{ccc}{r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}}\end{array}\right]^{T} \left[ \begin{array}{ccc}{\lambda_{1}} & {0} & {0} \\ {0} & {\lambda_{2}} & {0} \\ {0} & {0} & {\lambda_{3}}\end{array}\right] \left[ \begin{array}{ccc}{r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}}\end{array}\right]=\left[ \begin{array}{ccc}{a_{1}} & {a_{4} / 2} & {a_{5} / 2} \\ {a_{4} / 2} & {a_{2}} & {a_{6} / 2} \\ {a_{5} / 2} & {a_{6} / 2} & {a_{3}}\end{array}\right] M=RTBR=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤T⎣⎡λ1000λ2000λ3⎦⎤⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤=⎣⎡a1a4/2a5/2a4/2a2a6/2a5/2a6/2a3⎦⎤
可得球心的表达形式：
C=−12[a7,a8,a9](M)−1C=-\frac{1}{2}\left[a_{7}, a_{8}, a_{9}\right](M)^{-1} C=−21[a7,a8,a9](M)−1

其他参数：
SS=CMCT+1S S=C M C^{T}+1 SS=CMCT+1

椭球xxx轴长度：xscale=SSλ1x_{s c a l e}=\sqrt{\frac{S S}{\lambda_{1}}}xscale=λ1SS
椭球yyy轴长度：yscale=SSλ2y_{s c a l e}=\sqrt{\frac{S S}{\lambda_{2}}}yscale=λ2SS
椭球zzz轴长度：zscale=SSλ3z_{s c a l e}=\sqrt{\frac{S S}{\lambda_{3}}}zscale=λ3SS

接下来就是如何使用最小二乘法从测量数据中求出椭圆的9个参数。

最小二乘估计

下面举一个最小二乘估计的简单例子：

假设有下列rrr组观测数据
[(x1,y1),…(xr,yr)][(x_1,y_1),…(x_r,y_r)][(x1,y1),…(xr,yr)]

若待估计的形式为
y=ax2+bx+cy=ax^2+bx+cy=ax2+bx+c

则有

yi=(xi2xi1)(abc)\boldsymbol{y}_{i}=\left( \begin{array}{ccc}{\boldsymbol{x}_{i}^{2}} & {\boldsymbol{x}_{i}} & {1}\end{array}\right) \left( \begin{array}{l}{\boldsymbol{a}} \\ {\boldsymbol{b}} \\ {\boldsymbol{c}}\end{array}\right) yi=(xi2xi1)⎝⎛abc⎠⎞

即

Y=HKY=HKY=HK

H=(x12x11x22x21⋮⋮⋮xr2xr1)\boldsymbol{H}=\left( \begin{array}{ccc}{\boldsymbol{x}_{1}^{2}} & {\boldsymbol{x}_{1}} & {1} \\ {\boldsymbol{x}_{2}^{2}} & {\boldsymbol{x}_{2}} & {1} \\ {\vdots} & {\vdots} & {\vdots} \\ {\boldsymbol{x}_{r}^{2}} & {\boldsymbol{x}_{r}} & {1}\end{array}\right) H=⎝⎜⎜⎜⎛x12x22⋮xr2x1x2⋮xr11⋮1⎠⎟⎟⎟⎞

最优估计问题转换成线性方程组的求解。

当HHH列满秩时(即观测量数目大于待定参数数目时)，方程有解，此时左右同时乘HHH的最小二乘逆(左逆)inv(H)=(HTH)−1HTinv(H)=(H^T H)^{-1}H^Tinv(H)=(HTH)−1HT

那么

k=(abc)=(HTH)−1HTY\boldsymbol{k}=\left( \begin{array}{c}{\boldsymbol{a}} \\ {\boldsymbol{b}} \\ {\boldsymbol{c}}\end{array}\right)=\left(\boldsymbol{H}^{T} \boldsymbol{H}\right)^{-1} \boldsymbol{H}^{T} \boldsymbol{Y} k=⎝⎛abc⎠⎞=(HTH)−1HTY

最终得到线性方程组的解，即为最小二乘解(确定的kkk对所有的测量参数都适用，实际上，最小二乘解保证了误差的平方和最小，证明过程参见博文大疆笔试中的涉及矩阵最小二乘求解思路)

同样的，对磁力计椭球拟合来讲，其待估计的形式为：

a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1a_{1} x^{2}+a_{2} y^{2}+a_{3} z^{2}+a_{4} x y+a_{5} x z+a_{6} y z+a_{7} x+a_{8}y+a_{9} z=1a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1

可以写成如下形式

1=(x2y2z2xyxzyzxyz)(a1⋮a9)1=\left( \begin{array}{llllllllll}{x^{2}} & {y^{2}} & {z^{2}} & {x y} & {x z} & {y z} & {x} & {y} & {z}\end{array}\right) \left( \begin{array}{c}{a_{1}} \\ {\vdots} \\ {a_{9}}\end{array}\right) 1=(x2y2z2xyxzyzxyz)⎝⎜⎛a1⋮a9⎠⎟⎞

我们将所有的观测数据带入可得:

(11⋮1)=(x12y12z12⋯x1y1z1x22y22z22⋯x2y2z2⋮⋮⋮⋱⋮⋮⋮xr2yr2zr2⋯xryrzr)(a1a2⋮a9)\left( \begin{array}{c}{1} \\ {1} \\ {\vdots} \\ {1}\end{array}\right)=\left( \begin{array}{ccccccc}{x_{1}^{2}} & {y_{1}^{2}} & {z_{1}^{2}} & {\cdots} & {x_{1}} & {y_{1}} & {z_{1}} \\ {x_{2}^{2}} & {y_{2}^{2}} & {z_{2}^{2}} & {\cdots} & {x_{2}} & {y_{2}} & {z_{2}} \\ {\vdots} & {\vdots} & {\vdots} & {\ddots} & {\vdots} & {\vdots} & {\vdots} \\ {x_{r}^{2}} & {y_{r}^{2}} & {z_{r}^{2}} & {\cdots} & {x_{r}} & {y_{r}} & {z_{r}}\end{array}\right) \left( \begin{array}{c}{a_{1}} \\ {a_{2}} \\ {\vdots} \\ {a_{9}}\end{array}\right) ⎝⎜⎜⎜⎛11⋮1⎠⎟⎟⎟⎞=⎝⎜⎜⎜⎛x12x22⋮xr2y12y22⋮yr2z12z22⋮zr2⋯⋯⋱⋯x1x2⋮xry1y2⋮yrz1z2⋮zr⎠⎟⎟⎟⎞⎝⎜⎜⎜⎛a1a2⋮a9⎠⎟⎟⎟⎞

求上述方程的最小二乘解：

k=(a1a2⋮a9)=(HTH)−1HTYk=\left( \begin{array}{l}{a_{1}} \\ {a_{2}} \\ {\vdots} \\ {a_{9}}\end{array}\right)=\left(H^{T} H\right)^{-1} H^{T} Y k=⎝⎜⎜⎜⎛a1a2⋮a9⎠⎟⎟⎟⎞=(HTH)−1HTY

即可求得最优估计的椭圆参数。

matlab平台下的磁力计校正

将MPU9250的磁力计测量数据通过嵌入式平台导入上位机，再在matlab平台下进行椭球拟合，值得注意的是磁力计的测量数据应当尽量遍历空间中的各个指向，这样才能得到更精确的拟合效果(图中的采样数据较少)；如效果图所示，拟合算法基本能精确计算出椭球球心位置，这表示了磁力计三轴的偏移量，而实际飞控代码中也应对磁力计初始数据减去该偏移量。