这里主要介绍px4里面的定位模块，即EKF库。
x
=
[
q
v
p
Δ
θ
b
Δ
v
b
m
n
e
d
m
b
v
w
i
n
d
]
T
x={{\left[ \begin{matrix} q & v & p & \begin{matrix} \Delta {{\theta }_{b}} & \Delta {{v}_{b}} & {{m}_{ned}} & \begin{matrix} {{m}_{b}} & {{v}_{wind}} \\ \end{matrix} \\ \end{matrix} \\ \end{matrix} \right]}^{T}}
x=[qvpΔθbΔvbmnedmbvwind]T
其中
q
q
q表示NED系至体系的旋转四元数，
v
v
v表示在ned系的速度，
p
p
p表示机体系原点(imu位置）在ned系的位置,，
Δ
θ
b
\Delta {{\theta }_{b}}
Δθb表示陀螺仪角度增量的偏差，
Δ
v
b
\Delta {{v}_{b}}
Δvb为加速度计获取的速度增量偏差，
m
n
e
d
{{m}_{ned}}
mned为地磁向量在ned系的投影，
m
b
{{m}_{b}}
mb为磁力计的零偏，
v
w
i
n
d
{{v}_{wind}}
vwind为水平方向的风速。
Δ
θ
=
Δ
θ
m
-
Δ
θ
b
-
Δ
θ
e
a
r
t
h
+
η
\Delta \theta =\Delta {{\theta }_{m}}\text{-}\Delta {{\theta }_{b}}\text{-}\Delta {{\theta }_{earth}}\text{+}\eta
Δθ=Δθm-Δθb-Δθearth+η
上式中
Δ
θ
m
\Delta {{\theta }_{m}}
Δθm为最优传感器输出的结果（通过标定、圆锥补偿、温补），
Δ
θ
e
a
r
t
h
\Delta {{\theta }_{earth}}
Δθearth为地球自转运动所产生的角度增量，$\eta
为
均
值
为
0
，
方
差
为
d
a
V
a
r
的
高
斯
白
噪
声
。
将
为均值为0，方差为daVar的高斯白噪声。将
为均值为0，方差为daVar的高斯白噪声。将\Delta \theta
转
换
为
四
元
数
转换为四元数
转换为四元数\Delta {{q}_{k}}$得到，即；
q
k
+
1
=
q
k
⊗
Δ
q
k
{{q}_{k+1}}={{q}_{k}}\otimes \Delta {{q}_{k}}
qk+1=qk⊗Δqk
Δ v = Δ v m − Δ v b + η a \Delta v=\Delta {{v}_{m}}-\Delta {{v}_{b}}+{{\eta }_{a}} Δv=Δvm−Δvb+ηa
上式中
Δ
v
m
\Delta {{v}_{m}}
Δvm为最优加速度计传感器输出的结果（通过标定、圆锥补偿、温补），
η
a
{{\eta }_{a}}
ηa为均值为0，方差为dvVar的高斯白噪声。故而得到
v
k
+
1
=
v
k
+
C
n
b
Δ
v
+
[
0
;
0
;
g
]
Δ
t
{{v}_{k+1}}={{v}_{k}}+{{C}_{nb}}\Delta v+[0;0;g]\Delta t
vk+1=vk+CnbΔv+[0;0;g]Δt
p k + 1 = p k + ( v k + 1 + v k ) Δ t / 2 {{p}_{k+1}}={{p}_{k}}+({{v}_{k+1}}+{{v}_{k}})\Delta t/2 pk+1=pk+(vk+1+vk)Δt/2
3、状态方程、协方差方程递推
卡尔曼滤波的状态递推方程如下：
x
k
+
1
=
f
(
x
k
,
u
k
)
=
F
∗
x
k
+
G
∗
u
k
{{x}_{k+1}}=f({{x}_{k}},{{u}_{k}})=F*{{x}_{k}}+G*{{u}_{k}}
xk+1=f(xk,uk)=F∗xk+G∗uk
其中
u
k
{{u}_{k}}
uk为imu的测量信息，即
[
Δ
θ
m
,
Δ
v
m
]
[\Delta {{\theta }_{m}},\Delta {{v}_{m}}]
[Δθm,Δvm]。
当imu静止时，其加速度计敏感到的力为重力的反作用力，故其
a
m
=
−
C
b
n
[
0
;
0
;
g
]
{{a}_{m}}=-{{C}_{bn}}[0;0;g]
am=−Cbn[0;0;g]
C
b
n
=
[
cos
ψ
cos
θ
cos
ψ
sin
θ
−
cos
ϕ
sin
ψ
sin
ϕ
sin
ψ
+
cos
ϕ
cos
ψ
sin
θ
cos
θ
sin
ψ
cos
ϕ
cos
ψ
+
sin
ϕ
sin
ψ
sin
θ
cos
ϕ
sin
ψ
sin
θ
−
cos
ψ
sin
θ
−
sin
θ
cos
θ
sin
ϕ
cos
ϕ
cos
θ
]
T
{{C}_{bn}}={{\left[ \begin{matrix} \cos \psi \cos \theta & \cos \psi \sin \theta -\cos \phi \sin \psi & \sin \phi \sin \psi +\cos \phi \cos \psi \sin \theta \\ \cos \theta \sin \psi & \cos \phi \cos \psi +\sin \phi \sin \psi \sin \theta & \cos \phi \sin \psi \sin \theta -\cos \psi \sin \theta \\ -\sin \theta & \cos \theta \sin \phi & \cos \phi \cos \theta \\ \end{matrix} \right]}^{T}}
Cbn=⎣⎡cosψcosθcosθsinψ−sinθcosψsinθ−cosϕsinψcosϕcosψ+sinϕsinψsinθcosθsinϕsinϕsinψ+cosϕcosψsinθcosϕsinψsinθ−cosψsinθcosϕcosθ⎦⎤T
即可以得到初始俯仰角和滚转角
θ = a sin ( a m x / g ) \theta =a\sin ({{a}_{mx}}/g) θ=asin(amx/g)
ϕ = − a tan ( a m y / a m z ) \phi =-a\tan ({{a}_{my}}/{{a}_{mz}}) ϕ=−atan(amy/amz)
通过磁力计获取
建立临时体系，其坐标系为ned系旋转初始航向角所形成的坐标系
b
1
b_1
b1系，通过
m
b
1
=
C
b
1
b
m
b
{{m}^{b1}}={{C}_{b1b}}{{m}^{b}}
mb1=Cb1bmb即获取初始磁向量在
b
1
b_1
b1系的投影，即磁向量与
b
1
b_1
b1系x轴之间的夹角为
φ
1
=
−
a
tan
2
(
m
y
b
1
,
m
x
b
1
)
{{\varphi }_{1}}=-a\tan 2(m_{y}^{b1},m_{x}^{b1})
φ1=−atan2(myb1,mxb1)即真实航向角为
φ
=
φ
1
+
d
e
c
l
\varphi ={{\varphi }_{1}}+decl
φ=φ1+decl，其中
d
e
c
l
decl
decl为磁偏角，可通过查表获取。
通过视觉获取
由于通过视觉传感器信息可以获取导航系至体系的转换矩阵，其形式为
C
b
n
{{C}_{bn}}
Cbn，故得到
φ = a tan 2 ( C n b ( 2 , 1 ) , C n b ( 1 , 1 ) ) \varphi =a\tan 2({{C}_{nb}}(2,1),{{C}_{nb}}(1,1)) φ=atan2(Cnb(2,1),Cnb(1,1))
通过双天线获取
双天线单位矢量在体系的坐标为
ant
_
ve
c
b
=
[
cos
(
a
n
t
_
y
a
w
)
;
sin(ant
_
yaw);0
]
\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{b}}=[\cos (ant\_yaw);\text{sin(ant }\!\!\_\!\!\text{ yaw);0 }\!\!]\!\!\text{ }
ant _ vecb=[cos(ant_yaw);sin(ant _ yaw);0 ] ，其中
a
n
t
_
y
a
w
ant\_yaw
ant_yaw为双天线矢量与无人机纵轴之间的夹角，其方向为纵轴绕z轴进行顺时针旋转。
ant
_
ve
c
n
=
C
n
b
ant
_
ve
c
b
\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}={{C}_{nb}}\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{b}}
ant _ vecn=Cnbant _ vecb
即
φ
=
a
tan
2
(
ant
_
ve
c
n
(
2
)
,
ant
_
ve
c
n
(
1
)
)
−
a
n
t
_
y
a
w
\varphi =a\tan 2(\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}(2),\text{ant }\!\!\_\!\!\text{ ve}{{\text{c}}_{n}}(1))-ant\_yaw
φ=atan2(ant _ vecn(2),ant _ vecn(1))−ant_yaw
利用气压计或者外部视觉高度进行初始化，由_params.vdist_sensor_type进行控制。
由磁力计测得的磁矢量为
m
m
{{m}_{m}}
mm（已扣除先前估计的磁零偏值），
m
n
e
d
=
C
n
b
m
m
{{m}_{ned}}={{C}_{nb}}{{m}_{m}}
mned=Cnbmm
，其中
C
n
b
{{C}_{nb}}
Cnb由初始化所得到的姿态角获取。
其余状态全部初始化为0。
已知姿态角初始方差 R 0 {{R}_{0}} R0为sq(_params.initial_tilt_err)
q = f ( Φ , d Y a w ) = a n g l e 2 q u a t ( Φ ) ⊗ a n g l e 2 q u a t ( C b n [ 0 , 0 , d Y a w ] T ) q=f(\Phi ,dYaw)=angle2quat(\Phi )\otimes angle2quat({{C}_{bn}}{{[0,0,dYaw]}^{T}}) q=f(Φ,dYaw)=angle2quat(Φ)⊗angle2quat(Cbn[0,0,dYaw]T)
其中$\Phi $为姿态角,
d
Y
a
w
dYaw
dYaw为对准后的航向误差，其服从均值为0，方差为
R
d
Y
a
w
{{R}_{dYaw}}
RdYaw
（由对应航向传感器参数决定）。即
q
=
H
∗
Φ
+
G
∗
d
Y
a
w
q=H*\Phi +G*dYaw
q=H∗Φ+G∗dYaw，其中
H
=
∂
f
∂
Φ
∣
Φ
=
Φ
0
H=\frac{\partial f}{\partial \Phi }\left| \Phi ={{\Phi }_{0}} \right.
H=∂Φ∂f∣Φ=Φ0,
Φ
0
{{\Phi }_{0}}
Φ0为初始化姿态角，
G
=
∂
f
∂
d
Y
a
w
∣
d
Y
a
w
=
0
G=\frac{\partial f}{\partial dYaw}\left| dYaw=0 \right.
G=∂dYaw∂f∣dYaw=0。
即 q q q对应的协方差为 R q = H ∗ R 0 ∗ H T + G R d Y a w G T {{R}_{q}}=H*{{R}_{0}}*{{H}^{T}}+G{{R}_{dYaw}}{{G}^{T}} Rq=H∗R0∗HT+GRdYawGT
水平速度项噪声为 R v h = sq(fmaxf( _ params.gps _ vel _ noise, 0.01f)) {{R}_{vh}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.gps }\!\!\_\!\!\text{ vel }\!\!\_\!\!\text{ noise, 0}\text{.01f))} Rvh=sq(fmaxf( _ params.gps _ vel _ noise, 0.01f))
垂向速度项噪声为 R v v = 1.5 2 R v h {{R}_{vv}}={{1.5}^{2}}{{R}_{vh}} Rvv=1.52Rvh
水平位置项噪声为 R p h = sq(fmaxf( _ params.gps _ pos _ noise, 0.01f)) {{R}_{ph}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.gps }\!\!\_\!\!\text{ pos }\!\!\_\!\!\text{ noise, 0}\text{.01f))} Rph=sq(fmaxf( _ params.gps _ pos _ noise, 0.01f))
垂向位置项噪声 R p v R_{pv} Rpv则由不同高度源对应的标准差参数确定。
陀螺零偏的初始方差为
R
Δ
θ
b
=
sq(fmaxf(
_
params.switch
_
on
_
gyro
_
bias, 0.01f))
{{R}_{\Delta {{\theta }_{b}}}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.switch }\!\!\_\!\!\text{ on }\!\!\_\!\!\text{ gyro }\!\!\_\!\!\text{ bias, 0}\text{.01f))}
RΔθb=sq(fmaxf( _ params.switch _ on _ gyro _ bias, 0.01f))
加速度计零偏的初始方差为
R Δ v b = sq(fmaxf( _ params.switch _ on _ acc _ bias, 0.01f)) {{R}_{\Delta {{v}_{b}}}}=\text{sq(fmaxf( }\!\!\_\!\!\text{ params}\text{.switch }\!\!\_\!\!\text{ on }\!\!\_\!\!\text{ acc }\!\!\_\!\!\text{ bias, 0}\text{.01f))} RΔvb=sq(fmaxf( _ params.switch _ on _ acc _ bias, 0.01f))
其imu最优传感器切换时，即复位于上述值。
m n e d m_{ned} mned、 m b m_b mb的初始方差为 R m = s q ( _ params.mag _ noise ) {{R}_{m}}=sq(\text{ }\!\!\_\!\!\text{ params}\text{.mag }\!\!\_\!\!\text{ noise}) Rm=sq( _ params.mag _ noise)
v w i n d v_{wind} vwind的初始方差为 R v w i n d = s q ( _ params.initial _ wind _ uncertainty ) {{R}_{{{v}_{wind}}}}=sq(\text{ }\!\!\_\!\!\text{ params}\text{.initial }\!\!\_\!\!\text{ wind }\!\!\_\!\!\text{ uncertainty}) Rvwind=sq( _ params.initial _ wind _ uncertainty)
❤️ 扫一扫，添加我的公众号或者搜索【无人机开发】
