1.构建误差模型
设三轴加速度计的零偏误差与刻度误差分别为
o
x
,
o
y
,
o
z
,
s
x
,
s
y
,
s
z
o_x ,o_y, o_z, s_x, s_y ,s_z
ox,oy,oz,sx,sy,sz
某一时刻该三轴加速度计的测量值分别为
x
,
y
,
z
x,y,z
x,y,z
设当前时刻真实加速度向量为
a
=
(
a
x
,
a
y
,
a
z
)
a=(a_x,a_y,a_z)
a=(ax,ay,az)
有以下关系
(error function)
{
a
x
=
(
x
−
o
x
)
s
x
a
y
=
(
x
−
o
y
)
s
y
a
z
=
(
x
−
o
z
)
s
z
\begin{cases} a_x=(x-o_x)s_x \\ a_y=(x-o_y)s_y\\ az=(x-o_z)s_z \end{cases} \tag{error function}
⎩⎪⎨⎪⎧ax=(x−ox)sxay=(x−oy)syaz=(x−oz)sz(error function)
静止状态下,不管加速度计方向如何,其测量得到的加速度为重力加速度在各轴上的投影。而在地球上,重力加速度向量模值总是等于
1
g
1g
1g。假设
a
x
,
a
y
,
a
z
ax,ay,az
ax,ay,az单位为
g
g
g,便有以下等式成立:
a
x
2
+
a
y
2
+
a
z
2
=
1
a^2_x+a^2_y+a^2_z=1
ax2+ay2+az2=1
即:
(
x
−
o
x
)
2
s
x
2
+
(
x
−
o
y
)
2
s
y
2
+
(
x
−
o
z
)
2
s
z
2
=
1
(x-o_x)^2s^2_x+(x-o_y)^2s^2_y+(x-o_z)^2s^2_z=1
(x−ox)2sx2+(x−oy)2sy2+(x−oz)2sz2=1
至此,我们将加速度计的误差求解转化成为了一个数学问题:已知
x
,
y
,
z
x,y,z
x,y,z,由上一条等式,求解出
o
x
,
o
y
,
o
z
,
s
x
,
s
y
,
s
z
ox,oy,oz,sx,sy,sz
ox,oy,oz,sx,sy,sz
2.求解误差方程
上一节中,我们得到了传感器的误差方程:
(
x
−
o
x
)
2
s
x
2
+
(
x
−
o
y
)
2
s
y
2
+
(
x
−
o
z
)
2
s
z
2
=
1
(x-o_x)^2s^2_x+(x-o_y)^2s^2_y+(x-o_z)^2s^2_z=1
(x−ox)2sx2+(x−oy)2sy2+(x−oz)2sz2=1
求解方程,对于经历过九年义务教育的我们来说是一件非常熟悉的事情,但这里存在着几个棘手的问题:
- 该方程为六元二次方程,存在着6个未知数,如果只存在 x , y , z x,y,z x,y,z一组观测值,那将会有无穷多解。因此我们需要获得多组观测值 x i , y i , z i xi,yi,zi xi,yi,zi,以构建方程组,从而求出唯一解,满足方程组里的每一个方程
- 对于六元二次方程而言,使用六组独立的观测值,构建出包含6个方程的方程组,理论上可以求出方程组的唯一解。可是事实上,我们从传感器上获取到的观测值,都是带有一定随机噪声的,即观测值并非完全准确,从而导致求得的方程解未必精确
- 使用计算机,我们可以轻松求出线性方程组的唯一精确解(如高斯消元法),而非线性方程问题,无论是从理论上还是计算方法上,都要复杂得多,大部分时候,对于无特定形式的非线性方程 f ( x ) = 0 f(x)=0 f(x)=0,是没有办法直接求得精确解的。
为了解决上述问题,这里引进了一种数学方法:最小二乘法
最小二乘法(又称最小平方法)是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据,并使得这些求得的数据与实际数据之间误差的平方和为最小。
对于现实中的绝大部分问题,我们都是无法求得精确解的,于是退而求其次,使用最小二乘法,我们可以得到其近似解。
以传感器误差方程为例,设
r
r
r为某特定解下方程的误差,有:
r
=
1
−
(
x
−
o
x
)
2
s
x
2
+
(
x
−
o
y
)
2
s
y
2
+
(
x
−
o
z
)
2
s
z
2
r=1-(x-o_x)^2s^2_x+(x-o_y)^2s^2_y+(x-o_z)^2s^2_z
r=1−(x−ox)2sx2+(x−oy)2sy2+(x−oz)2sz2
对于方程组而言,其误差的平方和函数为
S
=
∑
i
=
0
n
r
i
2
S=\sum^n_{i=0}{r_i^2}
S=i=0∑nri2
所谓最小二乘法,便是求得一组特定解,使得
S
S
S最小(求函数极值),该条件下的解,便是方程组的最优近似解。对于线性
方程组,可将误差函数
S
=
0
S=0
S=0转换为矩阵方程并进行求解即可,可惜现实问题大部分均为非线性问题,如本文中的传感器误差方程便是一个非线性方程,是无法变换为矩阵方程形式的,因此我们必须换一个思路来解决问题。
假定该函数存在着局部最小值,那给定一个初始解,通过不断地迭代计算,从而找到误差平方和函数 S S S的局部最优解,这便是非线性最小二乘的基本思想。
接下来,我们将讨论求解非线性最小二乘的具体算法。