- 3.1 卡尔曼滤波
- 3.2 扩展卡尔曼滤波器
- 3.3 姿态/方向表示
- 3.3.1 欧拉角
- 3.3.2 方向余弦矩阵 C b n C_b^n Cbn
- 3.3.3 小角度方向余弦矩阵 C b n C_b^n Cbn
- 3.4 全球定位系统
- 3.5 GPS完好性监测
- 3.5.1 接收机自主完好性监测
- 3.5.2 奇偶向量
- 3.5.3 最小二乘残差
- 3.5.4 斜率方法
- 3.6 基于视觉的导航
- 3.6.1 投影模型
- 3.6.2 测量模型
- 3.7 当前视觉完好性监测
- 3.7.1 奇偶向量和斜率
卡尔曼滤波是将各种类型的传感器信息和系统知识以模型的形式结合起来，生成系统状态的最优估计的有效手段。名称过滤器通常用于净化某物（去除不需要的污染物）。从本质上说，卡尔曼滤波器是一个测量滤波器，滤除不需要的不确定性（测量噪声和模型噪声）。
卡尔曼滤波有两个不同的步骤，在离散时间的每个时刻内重复这两个步骤。第一步是预测/外推，利用系统模型预测一个时间间隔后系统的状态。第二步是观测/更新，在此期间，测量与预测结合使用，以估计该时间间隔内系统的状态。
线性系统通常使用状态空间表示进行建模，如连续时间系统见公式（2.1）和（2.2）所示，离散时间系统见公式（2.3）和（2.4）所示。
连续时间系统：
x
˙
(
t
)
=
F
x
(
t
)
+
ω
(
t
)
(2.1)
\dot{x}(t)=Fx(t)+\omega (t) \tag{2.1}
x˙(t)=Fx(t)+ω(t)(2.1)
y
(
t
)
=
C
x
(
t
)
+
v
(
t
)
(2.2)
y(t)=Cx(t)+v(t) \tag{2.2}
y(t)=Cx(t)+v(t)(2.2)
离散时间系统：
x
(
k
+
1
)
=
F
x
(
k
)
+
ω
(
k
)
(2.3)
x(k+1)=Fx(k)+\omega (k) \tag{2.3}
x(k+1)=Fx(k)+ω(k)(2.3)
z
(
k
)
=
ϕ
x
(
k
)
+
v
(
k
)
(2.4)
z(k)=\phi x(k)+v(k) \tag{2.4}
z(k)=ϕx(k)+v(k)(2.4)
系统模型和测量噪声/不确定度：
E
{
ω
(
k
)
}
=
0
(2.5)
E\{\omega (k)\} = 0 \tag{2.5}
E{ω(k)}=0(2.5)
E
{
ω
(
k
)
ω
T
(
k
)
}
=
Q
(
k
)
(2.6)
E\{\omega(k)\omega^T(k)\}=Q(k) \tag{2.6}
E{ω(k)ωT(k)}=Q(k)(2.6)
E
{
v
(
k
)
}
=
0
(2.7)
E\{v(k)\}=0\tag{2.7}
E{v(k)}=0(2.7)
E
{
v
(
k
)
v
T
(
k
)
}
=
R
(
k
)
(2.8)
E\{v(k)v^T(k)\}=R(k) \tag{2.8}
E{v(k)vT(k)}=R(k)(2.8)
预测/外推：卡尔曼滤波的这一步使用公式（2.10）和（2.11）外推状态估计和误差协方差矩阵。公式（2.10）通过使用一个粗略的状态空间模型
F
F
F预测系统的状态。然后利用模型
F
F
F和
Q
Q
Q将误差协方差矩阵更新为公式（2.11），用方差来描述模型的不确定性。
状态估计外推：
x
^
k
+
1
(
−
)
=
F
x
^
k
(
+
)
(2.10)
\hat{x}_{k+1}(-)=F\hat{x}_k(+) \tag{2.10}
x^k+1(−)=Fx^k(+)(2.10)
误差协方差外推：
P
k
+
1
(
−
)
=
F
P
k
(
+
)
F
T
+
Q
k
(2.11)
P_{k+1}(-)=FP_k(+)F^T+Q_k \tag{2.11}
Pk+1(−)=FPk(+)FT+Qk(2.11)
观测/更新：卡尔曼滤波的这一步使用公式（2.12）、（2.13）和（2.14）更新状态估计，
x
^
k
+
1
\hat{x}_{k+1}
x^k+1，包括测量/观测
z
k
+
1
z_{k+1}
zk+1，误差协方差矩阵
P
k
+
1
P_{k+1}
Pk+1和卡尔曼增益矩阵
K
K
K。观测和预测的结合是使用一种特殊的增益完成的，称为卡尔曼增益，
K
K
K。卡尔曼增益是基于不确定性的知识的。
状态估计更新：
x
^
k
+
1
(
+
)
=
x
^
k
+
1
(
−
)
+
K
k
+
1
[
z
k
+
1
−
H
k
+
1
x
^
k
+
1
(
−
)
]
(2.12)
\hat{x}_{k+1}(+)=\hat{x}_{k+1}(-)+K_{k+1}[z_{k+1}-H_{k+1}\hat{x}_{k+1}(-)] \tag{2.12}
x^k+1(+)=x^k+1(−)+Kk+1[zk+1−Hk+1x^k+1(−)](2.12)
误差协方差更新：
P
k
+
1
(
+
)
=
[
I
−
K
k
+
1
H
k
+
1
]
P
k
+
1
(
−
)
(2.13)
P_{k+1}(+)=[I-K_{k+1}H_{k+1}]P_{k+1}(-) \tag{2.13}
Pk+1(+)=[I−Kk+1Hk+1]Pk+1(−)(2.13)
卡尔曼增益：
K
k
=
P
k
(
−
)
H
k
T
[
H
k
P
k
(
−
)
H
k
T
+
R
k
]
−
1
(2.14)
K_k=P_k(-)H_k^T[H_kP_k(-)H_k^T+R_k]^{-1} \tag{2.14}
Kk=Pk(−)HkT[HkPk(−)HkT+Rk]−1(2.14)
原始卡尔曼滤波是一种很好的线性系统状态估计方法。不幸的是，并非所有的系统都是线性的。对于这些情况，有扩展卡尔曼滤波器。扩展卡尔曼滤波器（EKF）利用非线性模型来预测状态和测量值，并将状态估计模型线性化以计算协方差 P P P。
离散扩展卡尔曼滤波器由以下公式实现：
计算先验状态估计：
x
^
(
−
)
=
ϕ
k
−
1
(
x
^
k
−
1
(
+
)
)
(2.15)
\hat{x}(-)=\phi_{k-1}(\hat{x}_{k-1}(+)) \tag{2.15}
x^(−)=ϕk−1(x^k−1(+))(2.15)
计算预测测量：
z
^
k
=
h
k
(
x
^
k
(
−
)
)
(2.16)
\hat{z}_k=h_k(\hat{x}_k(-)) \tag{2.16}
z^k=hk(x^k(−))(2.16)
线性化系统方程：
Φ
k
−
1
[
1
]
≈
∂
ϕ
k
∂
x
∣
x
=
x
^
k
(
+
)
(2.17)
\Phi_{k-1}^{[1]} \approx \frac{\partial \phi_k}{\partial x}|_{x=\hat{x}_k(+)} \tag{2.17}
Φk−1[1]≈∂x∂ϕk∣x=x^k(+)(2.17)
将预测的估计值置于测量值之上：
x
^
k
(
+
)
=
x
^
k
(
−
)
+
K
ˉ
k
(
z
k
−
z
^
k
)
(2.18)
\hat{x}_k(+)=\hat{x}_k(-)+\bar{K}_k(z_k-\hat{z}_k) \tag{2.18}
x^k(+)=x^k(−)+Kˉk(zk−z^k)(2.18)
线性化测量方程：
H
k
−
1
[
1
]
≈
∂
h
k
∂
x
∣
x
=
x
^
k
(
−
)
(2.19)
H^{[1]}_{k-1}\approx \frac{\partial h_k}{\partial x}|_{x=\hat{x}_k(-)} \tag{2.19}
Hk−1[1]≈∂x∂hk∣x=x^k(−)(2.19)
计算先验协方差矩阵：
P
k
(
−
)
=
Φ
k
−
1
[
1
]
P
k
−
1
(
+
)
Φ
k
−
1
[
1
]
T
+
Q
k
−
1
(2.20)
P_k(-)=\Phi_{k-1}^{[1]}P_{k-1}(+)\Phi_{k-1}^{[1]T}+Q_{k-1} \tag{2.20}
Pk(−)=Φk−1[1]Pk−1(+)Φk−1[1]T+Qk−1(2.20)
计算卡尔曼增益：
K
ˉ
k
=
P
k
(
−
)
H
k
[
1
]
T
[
H
k
[
1
]
P
k
(
−
)
H
k
[
1
]
T
+
R
k
]
−
1
(2.21)
\bar{K}_k=P_k(-)H_k^{[1]T}[H_k^{[1]}P_k(-)H_k^{[1]T}+R_k]^{-1} \tag{2.21}
Kˉk=Pk(−)Hk[1]T[Hk[1]Pk(−)Hk[1]T+Rk]−1(2.21)
计算后验协方差矩阵：
P
k
(
+
)
=
{
I
−
K
ˉ
k
H
k
[
1
]
}
P
k
(
−
)
(2.22)
P_k(+)=\{I-\bar{K}_kH_k^{[1]}\}P_k(-) \tag{2.22}
Pk(+)={I−KˉkHk[1]}Pk(−)(2.22)
本文将用两种不同的方法来表示车辆的姿态。它们是欧拉角和旋转矩阵（
C
b
n
C_b^n
Cbn）。在载体系中，将使用捷联式IMU。来自它的数据将是关于载体系的，需要转换到导航系。这就是矩阵
C
b
n
C_b^n
Cbn所做的。它将向量从机体系（b）旋转到导航系（n）。这个旋转是用三个称为欧拉角（滚转角
ϕ
\phi
ϕ，俯仰角
θ
\theta
θ和偏航角
ψ
\psi
ψ）的角来完成的，并且是相对于载体系来完成的。
3.3.1 欧拉角
偏航/航向角
ψ
\psi
ψ：绕
z
z
z轴的旋转称为载体的航向角或偏航角。
z
z
z轴向下穿过载体的底部，围绕它旋转会改变导航系的移动方向。航向和偏航通常是相同的，除非载体处于侧滑状态，这可能是由风或机身动力学引起的。
R
(
ψ
)
=
[
c
o
s
(
ψ
)
s
i
n
(
ψ
)
0
−
s
i
n
(
ψ
)
c
o
s
(
ψ
)
0
0
0
1
]
=
C
1
n
(2.23)
\pmb{R}(\psi)=\begin{bmatrix} cos(\psi) & sin(\psi) & 0 \ -sin(\psi) & cos(\psi) & 0 \ 0 & 0 & 1 \end{bmatrix}=\pmb{C}_1^n \tag{2.23}
RRR(ψ)=⎣⎡cos(ψ)−sin(ψ)0sin(ψ)cos(ψ)0001⎦⎤=CCC1n(2.23)
俯仰角 θ \theta θ：围绕载体附着系（在这种情况下是第1帧）的 y y y轴旋转，在航空电子设备中被称为俯仰角。这是因为 y y y轴通常指向飞机机翼。绕 y y y轴旋转会使飞机的机头相对于地平线上下俯仰。
R ( θ ) = [ c o s ( θ ) 0 − s i n ( θ ) 0 1 0 s i n ( θ ) 0 c o s ( θ ) ] = C 2 1 (2.24) \pmb{R}(\theta)=\begin{bmatrix} cos(\theta) & 0 & -sin(\theta) \ 0 & 1 & 0 \ sin(\theta) & 0 & cos(\theta) \end{bmatrix}=\pmb{C}_2^1 \tag{2.24} RRR(θ)=⎣⎡cos(θ)0sin(θ)010−sin(θ)0cos(θ)⎦⎤=CCC21(2.24)
滚转角
ϕ
\phi
ϕ：围绕机体附着系（在这种情况下是第2帧）的
x
x
x轴旋转，在航空电子设备中被称为滚转。这是因为
x
x
x轴从机体质心的原点穿过机体的前部，沿着机体通常行驶的方向。对于飞机来说，这个旋转是围绕着机身的中心线。因为这是一个右手坐标系，顺时针旋转将是一个正的旋转角。
R
(
ϕ
)
=
[
1
0
0
0
c
o
s
(
ϕ
)
s
i
n
(
ϕ
)
0
−
s
i
n
(
ϕ
)
c
o
s
(
ϕ
)
]
=
C
b
2
(2.25)
\pmb{R}(\phi)=\begin{bmatrix} 1 & 0 & 0\ 0 & cos(\phi) & sin(\phi) \ 0 & -sin(\phi) & cos(\phi) \end{bmatrix}=\pmb{C}_b^2 \tag{2.25}
RRR(ϕ)=⎣⎡1000cos(ϕ)−sin(ϕ)0sin(ϕ)cos(ϕ)⎦⎤=CCCb2(2.25)
在这个表示方法中，旋转保持在矩阵形式。这个矩阵可以用欧拉角来计算，反之亦然。为了获得方向余弦矩阵，需要将欧拉角旋转放在一个序列中，这将创建一个旋转矩阵，将一个向量从导航系旋转到机体系。一个这样的序列和本文所使用的序列可以在公式（2.26）中看到，
C
b
n
=
R
(
ϕ
)
R
(
θ
)
R
(
ψ
)
=
[
c
11
c
12
c
13
c
21
c
22
c
23
c
31
c
32
c
33
]
(2.26)
\pmb{C}_b^n=\pmb{R}(\phi)\pmb{R}(\theta)\pmb{R}(\psi)=\begin{bmatrix} c_{11} & c_{12} & c_{13} \ c_{21} & c_{22} & c_{23} \ c_{31} & c_{32} & c_{33} \end{bmatrix} \tag{2.26}
CCCbn=RRR(ϕ)RRR(θ)RRR(ψ)=⎣⎡c11c21c31c12c22c32c13c23c33⎦⎤(2.26)
方向余弦矩阵（
C
b
n
C_b^n
Cbn）是一个3×3的正交阵；因此，它有如下性质：
C
b
n
C
b
n
T
=
I
3
×
3
(2.27)
\pmb{C}_b^n\pmb{C}_b^{nT}=\pmb{I}^{3\times 3} \tag{2.27}
CCCbnCCCbnT=III3×3(2.27)
d
e
t
(
C
b
n
)
=
1
(2.28)
det(\pmb{C}_b^n)=1 \tag{2.28}
det(CCCbn)=1(2.28)
公式（2.27）表示了正交矩阵的一个非常有用的性质，它的转置等于它的逆。公式（2.28）表示了方向余弦矩阵的法向性，它保证了当乘以一个向量时，结果只会被旋转，而不会被缩放。
C
b
n
=
[
c
o
s
(
θ
)
c
o
s
(
ψ
)
s
i
n
(
ϕ
)
s
i
n
(
θ
)
c
o
s
(
ψ
)
−
s
i
n
(
ψ
)
c
o
s
(
ϕ
)
c
o
s
(
ϕ
)
s
i
n
(
θ
)
c
o
s
(
ψ
)
+
s
i
n
(
ϕ
)
s
i
n
(
ψ
)
c
o
s
(
θ
)
s
i
n
(
ψ
)
c
o
s
(
ϕ
)
s
i
n
(
θ
)
s
i
n
(
ψ
)
−
c
o
s
(
ψ
)
c
o
s
(
ϕ
)
c
o
s
(
ϕ
)
s
i
n
(
θ
)
s
i
n
(
ψ
)
−
s
i
n
(
ϕ
)
c
o
s
(
ψ
)
−
s
i
n
(
θ
)
s
i
n
(
ϕ
)
c
o
s
(
θ
)
c
o
s
(
ϕ
)
c
o
s
(
θ
)
]
(2.29)
\pmb{C}_b^n=\begin{bmatrix} cos(\theta)cos(\psi) & sin(\phi)sin(\theta)cos(\psi)-sin(\psi)cos(\phi) & cos(\phi)sin(\theta)cos(\psi)+sin(\phi)sin(\psi) \ cos(\theta)sin(\psi) & cos(\phi)sin(\theta)sin(\psi)-cos(\psi)cos(\phi) & cos(\phi)sin(\theta)sin(\psi)-sin(\phi)cos(\psi) \ -sin(\theta) & sin(\phi)cos(\theta) & cos(\phi)cos(\theta) \end{bmatrix} \tag{2.29}
CCCbn=⎣⎡cos(θ)cos(ψ)cos(θ)sin(ψ)−sin(θ)sin(ϕ)sin(θ)cos(ψ)−sin(ψ)cos(ϕ)cos(ϕ)sin(θ)sin(ψ)−cos(ψ)cos(ϕ)sin(ϕ)cos(θ)cos(ϕ)sin(θ)cos(ψ)+sin(ϕ)sin(ψ)cos(ϕ)sin(θ)sin(ψ)−sin(ϕ)cos(ψ)cos(ϕ)cos(θ)⎦⎤(2.29)
对于小角度，公式（2.29）中的三角函数可采用一阶泰勒级数展开，例如
c
o
s
(
β
)
≈
1
cos(\beta)\approx1
cos(β)≈1和
s
i
n
(
β
)
≈
β
sin(\beta)\approx \beta
sin(β)≈β。将此应用到公式（2.29）中，得到一个反对称旋转矩阵，如公式（2.30）所示。
C
b
n
=
[
1
−
ψ
θ
ψ
1
−
ϕ
−
θ
ϕ
1
]
=
I
3
×
3
+
I
3
×
3
×
[
ϕ
θ
ψ
]
(2.30)
\pmb{C}_b^n=\begin{bmatrix} 1 & -\psi &\theta \ \psi & 1 & -\phi \ -\theta & \phi & 1 \end{bmatrix}=\pmb{I}^{3\times3}+\pmb{I}^{3\times3}\times\begin{bmatrix} \phi \ \theta \ \psi \end{bmatrix} \tag{2.30}
CCCbn=⎣⎡1ψ−θ−ψ1ϕθ−ϕ1⎦⎤=III3×3+III3×3×⎣⎡ϕθψ⎦⎤(2.30)
全球定位系统是由地球中轨道卫星组成的天基导航系统。它提供全球精确的三维位置和时间信息。GPS系统具有良好的长期精度，但由于高频噪声误差，短期精度较低，影响短期性能。这就是为什么它经常与惯性导航系统相结合的原因之一，惯性导航系统具有良好的短期精度，但会受到传感器误差造成的漂移的影响。组合系统提供了比单独使用更好的解决方案，并且可以在运行时降低对独立系统的性能要求。
GPS利用卫星发送的信号进行到达时间测量。由于用户只接收信号，它们被动地 *** 作，允许无限数量的用户同时使用。用户从已知位置的卫星接收信号。信号发送和接收的时间差乘以光速就可以确定到给定卫星的距离。利用多余的原子钟在卫星上精确地知道时间。然而，接收机没有配备准确的时钟。这导致无法准确地知道接收时间。因此，除位置外，时间也是需要求解的变量之一。由于时间是不精确的，接收机所做的测量称为伪距，因为接收机时间是未知的，所以它不是真实的距离。
由于本研究不针对GPS，因此在此不描述实际信号，但关于GPS信号的详细信息见参考文献[11,14,23,34]。从卫星发出的信号可以进行四种不同的测量。它们是伪距、多普勒、载波相位和载波噪声密度。这些测量是原始的，不应该与接收机产生的位置和速度的计算输出相混淆。大多数GPS辅助惯导方法需要从接收机获得原始测量值。最常用的测量方法是伪距，并且通常是唯一使用的测量方法。
伪距是用户于卫星之间的真实距离，加上由时间不确定性和其它误差源引起的偏差。产生偏差的主要原因是接收机时钟，但其它原因是卫星时钟、大气影响和多径干扰。伪距方程由下式给出，
ρ
=
r
+
c
(
δ
t
r
−
δ
t
s
)
+
c
δ
t
t
r
o
p
o
+
c
δ
t
i
o
n
o
+
c
δ
t
m
p
+
v
(2.31)
\rho=r+c(\delta t_r-\delta t_s)+c\delta t_{tropo} + c\delta t_{iono}+c\delta t_{mp}+v \tag{2.31}
ρ=r+c(δtr−δts)+cδttropo+cδtiono+cδtmp+v(2.31)
其中，
ρ
\rho
ρ是GPS伪距（单位米），
r
r
r是从用户到卫星之间的真实距离（单位米），
c
c
c是光速（单位米/秒），
δ
t
r
\delta t_r
δtr是接收机时钟误差（单位秒），
δ
t
s
\delta t_s
δts是卫星时钟误差（单位秒），
δ
t
t
r
o
p
o
\delta t_{tropo}
δttropo是对流层延迟造成的误差（单位秒），
δ
t
i
o
n
o
\delta t_{iono}
δtiono是电离层延迟造成的误差（单位秒），
δ
t
m
p
\delta t_{mp}
δtmp是多径干扰造成的误差（单位秒），
v
v
v是接收机噪声造成的误差（单位米）。
距离 r r r是卫星和接收机之间的真实视线距离。当信号在大气中传播时，信号的路径经常被扭曲，导致电离层和对流层产生误差，分别为 δ t i o n o \delta t_{iono} δtiono和 δ t t r o p o \delta t_{tropo} δttropo。大气模拟和预报可以缓解 δ t i o n o \delta t_{iono} δtiono和 δ t t r o p o \delta t_{tropo} δttropo的影响。当信号被物体和地面反射时，就会产生相同信号的多个副本。采用接收机和天线设计来减少多径影响，屏蔽所有不是真实的视线信号。通过减少这些误差，剩下的主导项来自接收机时钟。该误差可以建模为时钟偏差项，并在计算位置解时求解。其余的误差被定义为噪声。
由于距离
r
r
r是位置的非线性测量，接收机通过对位置的初始近似猜测来进行线性化，然后迭代求解来计算位置。这种位置求解方法在[23]中有一个完整的描述。伪距可简化为公式（2.32）。
ρ
=
(
x
m
i
−
x
t
)
2
+
(
y
m
i
−
y
t
)
2
+
(
z
m
i
−
z
t
)
2
+
b
+
ϵ
(2.32)
\rho=\sqrt{(x_{m_i}-x_t)^2+(y_{m_i}-y_t)^2+(z_{m_i}-z_t)^2}+b+\epsilon \tag{2.32}
ρ=(xmi−xt)2+(ymi−yt)2+(zmi−zt)2
+b+ϵ(2.32)
其中
(
x
m
i
,
y
m
i
,
z
m
i
)
(x_{m_i},y_{m_i},z_{m_i})
(xmi,ymi,zmi)是第
i
i
i颗卫星的位置（米），
(
x
t
,
y
t
,
z
t
)
(x_t,y_t,z_t)
(xt,yt,zt)是接收机真实的位置（米），
b
b
b是接收机时钟偏差（米），
ϵ
\epsilon
ϵ是测量中的误差（米）。
如果真实位置，
x
t
x_t
xt，和偏差项表示为
x
t
=
x
0
+
δ
x
x_t=x_0+\delta x
xt=x0+δx和
b
=
b
0
+
δ
b
b=b_0+\delta b
b=b0+δb，则误差项
δ
x
\delta x
δx和
δ
b
\delta b
δb表示对初始估计
x
0
x_0
x0和
b
0
b_0
b0进行的修正。如果
ρ
c
\rho_c
ρc是应用修正量
δ
x
\delta x
δx和
δ
b
\delta b
δb之后的伪距，则线性化方程用一阶泰勒级数近似得到，
δ
ρ
=
ρ
c
−
ρ
0
(2.33)
\delta \rho=\rho_c-\rho_0 \tag{2.33}
δρ=ρc−ρ0(2.33)
=
∣
∣
x
t
−
x
0
−
δ
x
∣
∣
−
∣
∣
x
t
−
x
0
∣
∣
+
(
b
−
b
0
)
+
ϵ
(2.34)
=||x_t-x_0-\delta x|| - || x_t - x_0 || + (b - b_0) + \epsilon \tag{2.34}
=∣∣xt−x0−δx∣∣−∣∣xt−x0∣∣+(b−b0)+ϵ(2.34)
≈
−
(
x
t
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
δ
x
+
δ
b
+
ϵ
(2.35)
\approx -\frac{(x_t-x_0)}{|| x_t - x_0 ||} \delta x + \delta b + \epsilon \tag{2.35}
≈−∣∣xt−x0∣∣(xt−x0)δx+δb+ϵ(2.35)
公式（2.35）写成矩阵
z
=
H
x
+
v
z=Hx+v
z=Hx+v形式有，
δ
ρ
=
[
δ
m
1
δ
m
2
⋮
δ
m
n
]
=
[
−
(
x
m
1
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
y
m
1
−
y
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
z
m
1
−
z
0
)
∣
∣
x
t
−
x
0
∣
∣
1
−
(
x
m
2
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
y
m
2
−
y
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
z
m
2
−
z
0
)
∣
∣
x
t
−
x
0
∣
∣
1
⋮
⋮
⋮
⋮
−
(
x
m
n
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
y
m
n
−
y
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
z
m
n
−
z
0
)
∣
∣
x
t
−
x
0
∣
∣
1
]
[
δ
x
δ
b
]
+
ϵ
(2.36)
\delta \rho=\begin{bmatrix} \delta_{m_1} \ \delta_{m_2} \ \vdots \ \delta_{m_n} \end{bmatrix}=\begin{bmatrix} -\frac{(x_{m_1}-x_0)}{|| x_t-x_0 ||} & -\frac{(y_{m_1}-y_0)}{|| x_t-x_0 ||} & -\frac{(z_{m_1}-z_0)}{|| x_t-x_0 ||} & 1\ -\frac{(x_{m_2}-x_0)}{|| x_t-x_0 ||} & -\frac{(y_{m_2}-y_0)}{|| x_t-x_0 ||} & -\frac{(z_{m_2}-z_0)}{|| x_t-x_0 ||} & 1\ \vdots & \vdots & \vdots & \vdots \ -\frac{(x_{m_n}-x_0)}{|| x_t-x_0 ||} & -\frac{(y_{m_n}-y_0)}{|| x_t-x_0 ||} & -\frac{(z_{m_n}-z_0)}{|| x_t-x_0 ||} & 1 \end{bmatrix} \begin{bmatrix} \delta x \ \delta b \end{bmatrix} + \epsilon \tag{2.36}
δρ=⎣⎢⎢⎢⎡δm1δm2⋮δmn⎦⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡−∣∣xt−x0∣∣(xm1−x0)−∣∣xt−x0∣∣(xm2−x0)⋮−∣∣xt−x0∣∣(xmn−x0)−∣∣xt−x0∣∣(ym1−y0)−∣∣xt−x0∣∣(ym2−y0)⋮−∣∣xt−x0∣∣(ymn−y0)−∣∣xt−x0∣∣(zm1−z0)−∣∣xt−x0∣∣(zm2−z0)⋮−∣∣xt−x0∣∣(zmn−z0)11⋮1⎦⎥⎥⎥⎥⎥⎤[δxδb]+ϵ(2.36)
公式（2.36）的解现在可以用线性数值方法，如最小二乘法来求解。超定方程组的最小二乘解由[23]给出，
[
δ
x
δ
b
]
=
(
H
T
H
)
−
1
H
T
δ
ρ
(2.37)
\begin{bmatrix} \delta x \ \delta b \end{bmatrix} = (\pmb{H}^T\pmb{H})^{-1}\pmb{H}^T \delta \rho \tag{2.37}
[δxδb]=(HHHTHHH)−1HHHTδρ(2.37)
其中，
H
=
[
−
(
x
m
1
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
y
m
1
−
y
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
z
m
1
−
z
0
)
∣
∣
x
t
−
x
0
∣
∣
1
−
(
x
m
2
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
y
m
2
−
y
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
z
m
2
−
z
0
)
∣
∣
x
t
−
x
0
∣
∣
1
⋮
⋮
⋮
⋮
−
(
x
m
n
−
x
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
y
m
n
−
y
0
)
∣
∣
x
t
−
x
0
∣
∣
−
(
z
m
n
−
z
0
)
∣
∣
x
t
−
x
0
∣
∣
1
]
(2.38)
\pmb{H}=\begin{bmatrix} -\frac{(x_{m_1}-x_0)}{|| x_t - x_0||} & -\frac{(y_{m_1}-y_0)}{|| x_t - x_0||} & -\frac{(z_{m_1}-z_0)}{|| x_t - x_0||} & 1 \ -\frac{(x_{m_2}-x_0)}{|| x_t - x_0||} & -\frac{(y_{m_2}-y_0)}{|| x_t - x_0||} & -\frac{(z_{m_2}-z_0)}{|| x_t - x_0||} & 1 \ \vdots & \vdots & \vdots & \vdots \ -\frac{(x_{m_n}-x_0)}{|| x_t - x_0||} & -\frac{(y_{m_n}-y_0)}{|| x_t - x_0||} & -\frac{(z_{m_n}-z_0)}{|| x_t - x_0||} & 1 \end{bmatrix} \tag{2.38}
HHH=⎣⎢⎢⎢⎢⎢⎡−∣∣xt−x0∣∣(xm1−x0)−∣∣xt−x0∣∣(xm2−x0)⋮−∣∣xt−x0∣∣(xmn−x0)−∣∣xt−x0∣∣(ym1−y0)−∣∣xt−x0∣∣(ym2−y0)⋮−∣∣xt−x0∣∣(ymn−y0)−∣∣xt−x0∣∣(zm1−z0)−∣∣xt−x0∣∣(zm2−z0)⋮−∣∣xt−x0∣∣(zmn−z0)11⋮1⎦⎥⎥⎥⎥⎥⎤(2.38)
这些方程用迭代最小二乘法来求解。重复该过程，直到修正量低于所设的阈值。
本节概述在GNSS系统中使用的完好性监测方法。本文首先讨论了接收机自主完好性监测（RAIM）。接下来是奇偶向量，以及获得检验统计量的最小二乘残差方法。最后讨论了斜率，即所使用的检验统计量与导航解的误差极限的关系。
3.5.1 接收机自主完好性监测GPS系统以其优良的性能和可靠性成为导航系统的首选。尽管它是一个相当可靠的系统，但在安全关键系统中使用它需要保证可靠性。这导致了大量的完好性监测算法的研究和开发，其中最重要的是接收机自主完好性监测（RAIM）。RAIM是迄今为止开发的最有用的方法，因为它是被动的和定位的GPS接收机，不需要大型和复杂的附加传感器基础设施。RAIM算法在接收机中不是标准化的，但它们主要依赖于来自特定数据瞬时的最小二乘残差或使用奇偶向量的类似方法。这些方法在检测和排除不良测量值方面有其局限性，它们假设存在单一测量故障，这对于GPS来说是一个有效的假设。
3.5.2 奇偶向量用于完好性监测的奇偶向量法是由Potter在[30]中首次提出的，用于监测惯性导航系统。随后，Sturza又将其作为GPS完好性监测的一种方法重新引入。下面是一个推导，反映了Sturza在数学细节上提出的一个推导。
奇偶向量法依赖于冗余测量的存在。换句话说，测量的数量
m
m
m，必须超过被估计的状态数
n
n
n，这样
m
−
n
≥
1
m-n\geq1
m−n≥1。线性化的测量模型为：
z
=
H
x
+
w
+
b
(2.39)
z = Hx + w +b \tag{2.39}
z=Hx+w+b(2.39)
其中
z
z
z是
m
×
1
m\times1
m×1维测量向量；
H
H
H是
m
×
n
m\times n
m×n维测量矩阵；
x
x
x是
n
×
1
n\times 1
n×1维状态向量；
w
w
w是
m
×
1
m\times 1
m×1测量噪声，它的协方差为
σ
2
I
\sigma^2I
σ2I；
b
b
b是
m
×
1
m\times1
m×1维偏差向量，它表示测量中的故障。假设存在冗余测量，
H
H
H并不是方阵。因此
H
H
H的Moore-Penrose伪逆为：
H
ˉ
=
(
H
T
H
)
−
1
H
T
(2.40)
\bar{H}=(H^TH)^{-1}H^T \tag{2.40}
Hˉ=(HTH)−1HT(2.40)
使用伪逆来求解
x
x
x产生的最小二乘估计为：
x
^
=
H
ˉ
z
=
(
H
T
H
)
−
1
H
T
(
H
x
+
w
+
b
)
=
x
+
H
ˉ
(
w
+
b
)
(2.41)
\hat{x}=\bar{H}z=(H^TH)^{-1}H^T(Hx+w+b)=x+\bar{H}(w+b) \tag{2.41}
x^=Hˉz=(HTH)−1HT(Hx+w+b)=x+Hˉ(w+b)(2.41)
假设测量矩阵
H
H
H由独立的列向量组成，使用QR分解可以成功分解，其中
Q
Q
Q是正交矩阵，而
R
R
R是上三角矩阵。
Q
T
z
=
R
x
(2.42)
Q^Tz=Rx \tag{2.42}
QTz=Rx(2.42)
最终导致
Q
Q
Q的维数是
m
×
m
m\times m
m×m，
R
R
R的维数是
m
×
n
m\times n
m×n最后
m
−
n
m-n
m−n行只包含0。
R
T
R^T
RT和
Q
Q
Q可以细分成
Q
x
T
Q_x^T
QxT和
U
U
U，分别表示
Q
T
Q^T
QT和
R
R
R的前
n
n
n行，而
Q
p
T
Q_p^T
QpT表示
Q
T
Q^T
QT的前
m
−
n
m-n
m−n行。
[
Q
x
T
Q
p
T
]
z
=
[
U
0
]
x
(2.43)
\begin{bmatrix} Q_x^T \ Q_p^T \end{bmatrix} z = \begin{bmatrix} U \ 0 \end{bmatrix}x \tag{2.43}
[QxTQpT]z=[U0]x(2.43)
矩阵
Q
p
T
Q_p^T
QpT定义为奇偶矩阵
P
P
P，它的行与
z
z
z正交，它的列跨越
H
H
H的奇偶空间。这允许带有不可观察偏差的测量转换到奇偶空间，在那里它们可以以奇偶向量的形式观察到：
p
=
P
z
=
P
(
w
+
b
)
(2.44)
p=Pz=P(w+b) \tag{2.44}
p=Pz=P(w+b)(2.44)
奇偶向量的结果元素为均值
μ
=
P
b
\mu=Pb
μ=Pb协方差为
σ
2
I
\sigma^2I
σ2I的正态分布。奇偶向量确实假设
p
p
p和
x
x
x是独立的，并且噪声
w
w
w的均值为零，当没有故障出现时，允许
p
p
p的均值为零。
除了奇偶向量
p
p
p，还可以将奇偶向量映射回测量空间，以故障向量
f
f
f的形式表示信息。一个变换矩阵
T
T
T可以通过将伪逆矩阵
H
ˉ
\bar{H}
Hˉ和奇偶矩阵
P
P
P增广得到。矩阵
T
T
T将测量空间映射到
n
n
n维状态空间和
m
−
n
m-n
m−n维奇偶空间。因此，
T
−
1
T^{-1}
T−1是从状态空间到测量空间的变换，形成故障向量
f
f
f，
f
=
T
−
1
[
0
p
]
=
[
H
ˉ
P
T
]
[
0
P
z
]
=
(
P
T
P
)
z
(2.45)
f=T^{-1}\begin{bmatrix} 0 \ p \end{bmatrix} = \begin{bmatrix} \bar{H} & P^T \end{bmatrix} \begin{bmatrix} 0 \ Pz \end{bmatrix} = (P^TP)z \tag{2.45}
f=T−1[0p]=[HˉPT][0Pz]=(PTP)z(2.45)
其中
f
f
f的结果元素服从正态分布，均值为
μ
=
P
T
P
b
\mu=P^TPb
μ=PTPb，协方差为
Σ
=
σ
2
P
T
P
\Sigma=\sigma^2P^TP
Σ=σ2PTP。奇偶向量
p
p
p和故障向量
f
f
f存在于不同的空间，但它们的内积得到的结果是相同的：
p
T
p
=
(
P
z
)
T
(
P
z
)
=
z
T
P
T
P
z
(2.46)
p^Tp=(Pz)^T(Pz)=z^TP^TPz \tag{2.46}
pTp=(Pz)T(Pz)=zTPTPz(2.46)
和
f
T
f
=
(
P
T
P
z
)
T
(
P
T
P
z
)
=
z
T
P
T
P
P
T
P
z
=
z
T
P
T
I
P
z
=
z
T
P
T
P
z
(2.47)
f^Tf=(P^TPz)^T(P^TPz)=z^TP^TPP^TPz=z^TP^TIPz=z^TP^TPz \tag{2.47}
fTf=(PTPz)T(PTPz)=zTPTPPTPz=zTPTIPz=zTPTPz(2.47)
该内积可作为故障检测的检验统计量
D
D
D。根据奇偶向量和故障向量的元素，决策变量
D
D
D服从卡方分布。在出现故障的情况下，
D
D
D的分布将成为一个非中心卡方分布，从而允许使用阈值检验来表明是否发生了故障。
H
0
:
D
<
λ
H_0:\ \ D < \lambda
H0: D<λ
H
1
:
D
>
λ
H_1: \ \ D > \lambda
H1: D>λ
该决策变量接受双假设检验，其中
H
0
H_0
H0表示无故障，
H
1
H_1
H1表示有故障。这是通过将
D
D
D与阈值
γ
\gamma
γ进行比较来实现的，阈值
γ
\gamma
γ是基于理想的虚警概率
p
f
a
p_{fa}
pfa、冗余测量数目
m
−
n
m-n
m−n和测量噪声的协方差
σ
2
\sigma^2
σ2。
γ
=
σ
2
e
r
f
c
−
1
(
p
f
a
m
−
n
)
(2.48)
\gamma = \sigma \sqrt{2} erfc^{-1}(\frac{p_{fa}}{m-n}) \tag{2.48}
γ=σ2
erfc−1(m−npfa)(2.48)
p
f
a
=
e
r
f
c
(
γ
σ
2
)
(2.49)
p_{fa}=erfc(\frac{\gamma}{\sigma \sqrt{2}}) \tag{2.49}
pfa=erfc(σ2
γ)(2.49)
[1,7,19,22,29]所做的工作为利用最小二乘残差进行GPS完好性监测奠定了基础。最小二乘残差的假设与奇偶空间法的假设相同，即存在冗余测量值，使得系统超定，测量值数目
m
m
m大于状态数目
n
n
n，即
m
−
n
≥
1
m-n\geq1
m−n≥1。对于GPS，
n
=
4
n=4
n=4，因为要求解的状态包括三维位置和一个时钟偏差项。在提出最小二乘残差法的初始工作中，测量方程是根据伪距给出的：
ρ
i
=
d
i
−
[
e
i
T
1
]
x
−
ϵ
i
(2.50)
\rho_i=d_i-\begin{bmatrix} e_i^T & 1 \end{bmatrix}x - \epsilon_i \tag{2.50}
ρi=di−[eiT1]x−ϵi(2.50)
其中
ρ
i
\rho_i
ρi是第
i
i
i颗卫星的伪距，
d
i
d_i
di是用户与第
i
i
i颗卫星之间的距离，
e
i
\pmb{e}_i
eeei是用户到第
i
i
i颗卫星的单位向量，
x
\pmb{x}
xxx是包含位置和时钟偏差项的状态向量，
ϵ
i
\epsilon_i
ϵi是测量噪声，它服从均值为
μ
i
\mu_i
μi方差为
σ
i
2
\sigma_i^2
σi2的正态分布。测量方程的向量表示为：
z
=
d
−
ρ
=
[
e
i
T
1
⋮
⋮
e
m
T
1
]
x
+
ϵ
=
H
x
+
ϵ
(2.51)
z = d - \rho = \begin{bmatrix} e_i^T & 1 \ \vdots & \vdots \ e_m^T & 1 \end{bmatrix}\pmb{x} +\epsilon=H\pmb{x}+\epsilon \tag{2.51}
z=d−ρ=⎣⎢⎡eiT⋮emT1⋮1⎦⎥⎤xxx+ϵ=Hxxx+ϵ(2.51)
最小二乘估计表示为：
x
^
(
H
T
H
)
−
1
H
T
z
=
H
ˉ
z
(2.52)
\hat{x}(H^TH)^{-1}H^Tz=\bar{H}z \tag{2.52}
x^(HTH)−1HTz=Hˉz(2.52)
估计的测量为：
z
^
=
H
x
^
=
H
H
ˉ
z
(2.53)
\hat{z} =H\hat{x}=H\bar{H}z \tag{2.53}
z^=Hx^=HHˉz(2.53)
实际测量值与预测测量值之间的差值就产生了残差向量：
ϵ
^
=
z
−
z
^
=
(
I
−
H
H
ˉ
)
z
(2.54)
\hat{\epsilon}=z-\hat{z}=(I-H\bar{H})z \tag{2.54}
ϵ^=z−z^=(I−HHˉ)z(2.54)
将公式（2.51）代入上式可得：
ϵ
^
=
(
I
−
H
H
ˉ
)
(
H
x
+
ϵ
)
(2.55)
\hat{\epsilon}=(I-H\bar{H})(Hx+\epsilon) \tag{2.55}
ϵ^=(I−HHˉ)(Hx+ϵ)(2.55)
=
H
x
−
H
H
ˉ
H
x
+
ϵ
−
H
H
ˉ
ϵ
(2.56)
=Hx-H\bar{H}Hx+\epsilon-H\bar{H}\epsilon \tag{2.56}
=Hx−HHˉHx+ϵ−HHˉϵ(2.56)
=
H
(
I
−
H
ˉ
H
)
x
+
(
I
−
H
H
ˉ
)
ϵ
(2.57)
=H(I-\bar{H}H)x+(I-H\bar{H})\epsilon \tag{2.57}
=H(I−HˉH)x+(I−HHˉ)ϵ(2.57)
=
H
(
I
−
(
H
T
H
)
−
1
H
T
H
)
x
+
(
I
−
H
H
ˉ
)
ϵ
(2.58)
=H(I-(H^TH)^{-1}H^TH)x+(I-H\bar{H})\epsilon \tag{2.58}
=H(I−(HTH)−1HTH)x+(I−HHˉ)ϵ(2.58)
=
H
(
I
−
I
)
x
+
(
I
−
H
H
ˉ
)
ϵ
(2.59)
=H(I-I)x+(I-H\bar{H})\epsilon \tag{2.59}
=H(I−I)x+(I−HHˉ)ϵ(2.59)
=
(
I
−
H
H
ˉ
)
ϵ
(2.60)
=(I-H\bar{H})\epsilon \tag{2.60}
=(I−HHˉ)ϵ(2.60)
定义误差平方和（SSE）为
ϵ
^
\hat{\epsilon}
ϵ^的内积，是一个有用的统计检验量。在噪声均值为零（无故障偏差）的情况下，SSE具有卡方分布，就像奇偶向量法中的决策变量一样。自由度为
m
−
4
m-4
m−4时，统计检验量为：
r
=
ϵ
^
T
ϵ
^
m
−
4
=
S
S
E
m
−
4
(2.61)
r=\sqrt{\frac{\hat{\epsilon}^T\hat{\epsilon}}{m-4}}=\sqrt{\frac{SSE}{m-4}} \tag{2.61}
r=m−4ϵ^Tϵ^
=m−4SSE
(2.61)
如果测量值受到噪声中非零均值的影响，则统计检验量将来自非中心卡方分布，其非中心参数
λ
\lambda
λ给定为：
λ
=
(
ρ
b
i
a
s
σ
)
2
(2.62)
\lambda=(\frac{\rho_{bias}}{\sigma})^2 \tag{2.62}
λ=(σρbias)2(2.62)
非中心卡方分布不能用解析形式表示，但可以用数值积分来近似。完好性检查过程通过将统计检验量
r
r
r与阈值
γ
\gamma
γ进行比较来执行。阈值
γ
\gamma
γ是通过蒙特卡罗模拟产生的，并根据需要的虚警和漏警要求进行选择。
斜率方法与奇偶向量法或最小二乘残差法联合使用，在将统计检验量与有偏测量引起的位置误差联系起来时很有用。“斜率”是水平位置误差与检验统计量的比值。对于GPS，“斜率”是一个线性关系，近似于伪距偏差的增长及其对水平位置误差的影响。对于任何给定的卫星，“斜率”是水平位置空间与奇偶空间之间的函数，由下式给出，
S
l
o
p
e
i
=
H
ˉ
1
,
i
2
+
H
ˉ
2
,
i
2
S
i
,
i
(2.63)
Slope_i=\frac{\sqrt{\bar{H}_{1,i}^2+\bar{H}_{2,i}^2}}{\sqrt{S_{i,i}}} \tag{2.63}
Slopei=Si,i
Hˉ1,i2+Hˉ2,i2
(2.63)
其中
S
=
P
T
P
S=P^TP
S=PTP。该方法假设没有噪声，只考虑偏差的影响；因此，奇偶向量为
p
=
P
(
b
+
0
)
p=P(b+0)
p=P(b+0)。第
i
i
i次测量得到的水平位置误差为：
H
ϵ
i
=
S
l
o
p
e
i
∣
∣
P
∣
∣
(2.64)
H_{\epsilon i}=Slope_i || P|| \tag{2.64}
Hϵi=Slopei∣∣P∣∣(2.64)
每次测量的斜率将根据测量几何形状的不同而不同。用最大斜率的测量值可以估计出最坏情况的上界，给出如下：
∣
H
b
i
a
s
∣
=
m
a
x
i
=
1
:
m
[
S
l
o
p
e
i
]
∣
∣
p
∣
∣
(2.65)
|H_{bias}|=\underset{i=1:m}{max}[Slope_i]||p|| \tag{2.65}
∣Hbias∣=i=1:mmax[Slopei]∣∣p∣∣(2.65)
由于该过程不是真正确定的，并且系统中存在测量噪声，因此该估计不能反映真实的水平位置误差。然而，假设测量噪声为零均值加性高斯白噪声，使得确定性方法成为一种合理的近似奇偶向量期望值的方法。斜率法允许将位置误差投影到奇偶空间上，允许相对于保护水平容易地显示阈值。
本研究的目的是进一步研究视觉辅助或基于视觉的导航系统中的完好性监测技术。因此，重点是视觉系统在已知特征的图像中以像素坐标的形式进行的测量。以下包括与真实世界目标/特征和视觉系统测量相关的基本背景。这项研究的重点是由成像系统提供的测量，而不是创建它的过程。因此，我们做了以下假设。
- 特征跟踪器以合适的速率以已知特征的像素坐标的形式生成测量值。
- 所述视觉系统允许已知像素坐标和相机系中位置之间的关系以及已经校正的任何镜头畸变。
- 相机系与机体系之间的关系是已知的。
全球定位系统与惯导系统的结合已经得到很好的应用。这两种系统相互配合得很好，但有些环境和条件会导致GPS信号不可用。这导致了对使用光学系统辅助导航的研究。基于视觉的导航可以在不使用INS的情况下完成，但视觉系统的性能是基于测量的质量，可以在给定的环境和跟踪特征的可用性基础上进行。当与INS紧耦合时，视觉系统可以以类似GPS的方式使用，以限制随时间增长的误差。INS和视觉系统结合在一起，有可能可靠地提供GPS级别的精度。
光学导航可以用于许多环境，包括那些未知的环境。当环境未知且已知位置没有特征时，采用同时定位与建图（SLAM）来估计可跟踪特征的位置，求解车辆的导航状态。本文提出的研究重点是在已知位置具有已知特征的环境中进行导航。这些图像将由一个被动的视觉系统跟踪，它接收一个三维场景，并将其投影到一个二维图像平面上。
3.6.1 投影模型
相机的光学特性决定了场景及其在图像上的投影之间的关系。对于一个简单的模型，光学很少表现出理想的性质。然而，许多校正和修正技术的存在，以减少和校正非线性光学效应。这些修正允许投影成为一个理想的薄透镜模型。对于一个理想的薄透镜，图像平面中的投影是焦距和与透镜之间的距离的函数，如图2.6所示。薄透镜将平行光线引向焦点，从而在焦点之外形成倒转的图像。这表示为薄透镜方程的基本方程为，
1
Z
+
1
z
=
1
f
(2.66)
\frac{1}{Z}+\frac{1}{z}=\frac{1}{f} \tag{2.66}
Z1+z1=f1(2.66)
其中
Z
Z
Z是场景中物体到镜头的距离，
z
z
z是透镜到图像平面之间的距离，
f
f
f是透镜的焦距。如果镜头的光圈变小，可以模拟成针孔相机。根据图2.6所示的针孔相机模型，所有光线必须通过光圈，并将图像投影到距光圈焦距
f
f
f处的平面上。
如果将图像平面放置在光学中心前面，则模型进一步简化，如图2.7所示。这就得到了一个不倒转的图像。给定一个点源位置，
s
c
s^c
sc相对于光学中心，得到的图像平面上的位置由下式给出，
s
p
r
o
j
=
(
f
s
z
c
)
s
c
(2.67)
s^{proj}=(\frac{f}{s_z^c})s^c \tag{2.67}
sproj=(szcf)sc(2.67)
以
s
z
c
s_z^c
szc为距相机光学中心
z
c
z_c
zc方向的距离。然后，这种相机投影被转换成数字图像。图像平面坐标需要映射到基于像素的坐标系。假设有一个高
H
H
H宽
W
W
W的矩形（
M
×
N
M\times N
M×N）像素网格，则投影坐标到像素坐标的转换为，
s
p
i
x
=
[
−
M
H
0
0
0
N
W
0
]
s
p
r
o
j
+
[
M
+
1
2
N
+
1
2
]
(2.68)
s^{pix}=\begin{bmatrix} -\frac{M}{H} & 0 & 0 \ 0 & \frac{N}{W} & 0 \end{bmatrix} s^{proj} + \begin{bmatrix} \frac{M+1}{2} \ \frac{N+1}{2} \end{bmatrix} \tag{2.68}
spix=[−HM00WN00]sproj+[2M+12N+1](2.68)
结合公式（2.67）和公式（2.68）可以得到从相机系到像素系的变换：
s
p
i
x
=
1
s
z
c
[
−
f
M
H
0
M
+
1
2
0
f
N
W
N
+
1
2
0
0
1
]
s
c
(2.69)
s^{pix}=\frac{1}{s_z^c}\begin{bmatrix} -f\frac{M}{H} & 0 & \frac{M+1}{2} \ 0 & f\frac{N}{W} & \frac{N+1}{2} \ 0 & 0 & 1 \end{bmatrix} s^c \tag{2.69}
spix=szc1⎣⎡−fHM000fWN02M+12N+11⎦⎤sc(2.69)
=
1
s
z
2
T
c
p
i
x
s
c
(2.70)
=\frac{1}{s_z^2}T_c^{pix}s^c \tag{2.70}
=sz21Tcpixsc(2.70)
其中
T
c
p
i
x
T_c^{pix}
Tcpix是相机到像素的齐次变换矩阵。要将像素坐标转换回相机坐标，可以使用逆变换如下：
T
p
i
x
c
=
(
T
c
p
i
x
)
−
1
(2.71)
T^c_{pix}=(T^{pix}_c)^{-1} \tag{2.71}
Tpixc=(Tcpix)−1(2.71)
T p i x c = [ − H f M 0 H ( M + 1 ) 2 f M 0 W f N − W ( N + 1 ) 2 f N 0 0 1 ] (2.72) T_{pix}^c=\begin{bmatrix} -\frac{H}{fM} & 0 & \frac{H(M+1)}{2fM} \ 0 & \frac{W}{fN} & -\frac{W(N+1)}{2fN} \ 0 & 0 & 1 \end{bmatrix} \tag{2.72} Tpixc=⎣⎢⎡−fMH000fNW02fMH(M+1)−2fNW(N+1)1⎦⎥⎤(2.72)
坐标仍然是一个相机模型，需要与导航系相关，如图2.8所示。导航系与相机系之间的关系由下式给出，
s
n
=
t
n
−
p
n
(2.73)
s^n=t^n-p^n \tag{2.73}
sn=tn−pn(2.73)
s
c
=
C
b
c
C
n
b
s
n
(2.74)
s^c=C_b^cC_n^bs^n \tag{2.74}
sc=CbcCnbsn(2.74)
其中
s
n
s^n
sn和
s
c
s^c
sc分别为导航系和相机系中相机到目标的视线向量，
p
n
p^n
pn为相机在导航系中的位置，
t
n
t^n
tn为目标在导航系中的位置。
在可以使用或分析测量之前，有必要有一个线性化的测量模型。本研究利用Veth创建的模型。对于该模型，使用了视觉辅助惯性系统的最小误差状态向量，如
δ
x
=
[
δ
p
n
δ
ψ
]
(2.75)
\delta x = \begin{bmatrix} \delta p^n \ \delta \psi \end{bmatrix} \tag{2.75}
δx=[δpnδψ](2.75)
其中
δ
x
\delta x
δx是误差状态向量，
δ
p
n
\delta p^n
δpn是平台位置的三维误差向量，
δ
ψ
\delta \psi
δψ是倾斜误差状态。
给出了第
i
i
i个图像特征的测量模型，
z
i
=
s
i
p
i
x
=
T
c
p
i
x
s
i
c
(2.76)
z_i=s_i^{pix}=T_c^{pix}s_i^c \tag{2.76}
zi=sipix=Tcpixsic(2.76)
其中
z
i
z_i
zi是第
i
i
i个特征的测量向量，
T
c
p
i
x
T_c^{pix}
Tcpix是从相机系到像素系的齐次变换矩阵，
s
i
c
s_i^c
sic是从相机到第
i
i
i个特征目标的视线向量。这是一个非线性关系，用非线性测量方程
h
(
x
)
h(x)
h(x)表示为，
h
(
x
)
=
T
c
p
i
x
s
i
c
(2.77)
h(x)=T_c^{pix}s_i^c \tag{2.77}
h(x)=Tcpixsic(2.77)
测量模型矩阵由
h
(
x
)
h(x)
h(x)的一阶泰勒级数展开得到。取
μ
=
1
/
s
z
c
\mu=1/s_z^c
μ=1/szc和
β
=
[
0
0
1
]
\beta=[0\ 0\ 1]
β=[0 0 1]，则测量矩阵由下式给出，
H
=
∂
h
(
x
)
∂
x
∣
x
=
x
^
=
[
∂
h
∂
p
n
∂
h
∂
ψ
]
(2.78)
H=\frac{\partial h(x)}{\partial x}|_{x=\hat{x}}=[\frac{\partial h}{\partial p^n}\ \frac{\partial h}{\partial \psi}] \tag{2.78}
H=∂x∂h(x)∣x=x^=[∂pn∂h ∂ψ∂h](2.78)
∂ h ∂ p n = μ T c p i x ( s i c β C b c C n b − C b c C n b ) (2.79) \frac{\partial h}{\partial p^n}=\mu T_c^{pix}(s_i^c\beta C_b^cC_n^b-C_b^cC_n^b) \tag{2.79} ∂pn∂h=μTcpix(sicβCbcCnb−CbcCnb)(2.79)
∂ h ∂ ψ = μ T c p i x ( ∂ s i c ∂ ψ − s i c β ∂ s i c ∂ ψ ) (2.80) \frac{\partial h}{\partial \psi}=\mu T_c^{pix}(\frac{\partial s_i^c}{\partial \psi}-s_i^c\beta \frac{\partial s_i^c}{\partial \psi}) \tag{2.80} ∂ψ∂h=μTcpix(∂ψ∂sic−sicβ∂ψ∂sic)(2.80)
∂ s i c ∂ ψ = − C b c C n b [ ( t n − p n ) × ] (2.81) \frac{\partial s_i^c}{\partial \psi}=-C_b^cC_n^b[(t^n-p^n)\times] \tag{2.81} ∂ψ∂sic=−CbcCnb[(tn−pn)×](2.81)
3.7 当前视觉完好性监测视觉导航领域可以细分为两类：基于跟踪已知特征的方法和基于跟踪未知特征的方法。因此，在执行完好性监测时，需要对测量进行不同的处理。图2.9显示了视觉系统完好性监测领域的细分。
在视觉导航系统完好性监测方面的研究还很少。最初的框架由Larson在[16-18]中提出。本文将基于奇偶向量和斜率的GPS完好性监测方法引入到视觉导航系统中。这项工作的重点是检测与已知特征相关的单个像素对误差，如图2.9所示。下面是这项工作的摘要。
3.7.1 奇偶向量和斜率本节包括[16-18]中所做工作的推导。这种推导的提供方式与[16]中相同。在这项工作中，提出了以下四个假设：
- 被跟踪的特征是已知的，不需要估计。
- 一个基于图像的测量被认为是一个由 ( x , y ) (x,y) (x,y)对组成的两个元素集。
- 偏差是多维的，因为它是误差在 x x x和 y y y方向上的角度的大小乘以正弦分量。
- 假设噪声为零均值加性高斯白噪声。
这种推导利用了这样一个事实：像素对
x
x
x和
y
y
y元素是由单个测量连接起来的测量值，在测量向量中保持相邻的位置
i
i
i和
j
j
j。偏差向量
b
b
b的组成成分为
b
i
=
∣
∣
b
∣
∣
s
i
n
θ
b_i=||b||sin\theta
bi=∣∣b∣∣sinθ和
b
j
=
∣
∣
b
∣
∣
c
o
s
θ
b_j=||b||cos\theta
bj=∣∣b∣∣cosθ，其中
∣
∣
b
∣
∣
||b||
∣∣b∣∣是像素误差的大小，
θ
\theta
θ是
x
−
y
x-y
x−y像素系中误差的角度。[2]中描述的斜率法是水平位置误差的平方向量模
∣
∣
δ
x
∣
∣
2
||\delta x||^2
∣∣δx∣∣2与奇偶向量的平方向量模
∣
∣
p
∣
∣
2
||p||^2
∣∣p∣∣2之比，如果使用残差法，则为残差向量。如果使用奇偶向量法，得到的结果关系为，
∣
∣
δ
x
h
∣
∣
2
∣
∣
p
∣
∣
2
=
δ
x
T
δ
x
p
T
p
=
b
T
H
ˉ
h
T
H
ˉ
h
b
b
T
P
T
P
b
(2.82)
\frac{||\delta x_h||^2}{||p||^2}=\frac{\delta x^T\delta x}{p^Tp}=\frac{b^T\bar{H}_h^T\bar{H}_hb}{b^TP^TPb} \tag{2.82}
∣∣p∣∣2∣∣δxh∣∣2=pTpδxTδx=bTPTPbbTHˉhTHˉhb(2.82)
其中
H
ˉ
=
(
H
T
H
)
−
1
H
T
\bar{H}=(H^TH)^{-1}H^T
Hˉ=(HTH)−1HT，它是
H
H
H的Moore-Penrose伪逆；
P
P
P是上一节描述的奇偶矩阵。
δ
x
h
\delta x_h
δxh和
H
ˉ
h
\bar{H}_h
Hˉh的下标
h
h
h表示它只包含
δ
x
\delta x
δx的水平位置元素和
H
ˉ
\bar{H}
Hˉ的对应行。公式（2.82）使用
G
=
H
ˉ
h
T
H
ˉ
h
G=\bar{H}_h^T\bar{H}_h
G=HˉhTHˉh和
S
=
P
T
P
S=P^TP
S=PTP可以进一步简化为，
∣
∣
δ
x
∣
∣
2
∣
∣
p
∣
∣
2
=
b
T
G
b
b
T
S
b
(2.83)
\frac{||\delta x||^2}{||p||^2}=\frac{b^TGb}{b^TSb} \tag{2.83}
∣∣p∣∣2∣∣δx∣∣2=bTSbbTGb(2.83)
假设只有一个误差，使除
b
i
b_i
bi和
b
j
b_j
bj外的所有元素的偏差向量为零，则比值的分子为，
b
T
G
b
=
b
i
2
G
i
i
+
b
i
b
j
(
G
i
j
+
G
j
i
)
+
b
j
2
G
j
j
(2.84)
b^TGb=b_i^2G_{ii}+b_ib_j(G_{ij}+G_{ji})+b_j^2G_{jj} \tag{2.84}
bTGb=bi2Gii+bibj(Gij+Gji)+bj2Gjj(2.84)
由于
G
G
G是对称的，即
G
i
j
=
G
j
i
G_{ij}=G_{ji}
Gij=Gji，将
b
i
b_i
bi和
b
j
b_j
bj的正弦定义代入得到，
b
T
G
b
=
∣
∣
b
∣
∣
2
s
i
n
2
(
θ
)
G
i
i
+
2
∣
∣
b
∣
∣
2
s
i
n
(
θ
)
c
o
s
(
θ
)
G
j
i
+
∣
∣
b
∣
∣
2
c
o
s
2
(
θ
)
G
j
j
(2.85)
b^TGb=||b||^2sin^2(\theta)G_{ii}+2||b||^2sin(\theta)cos(\theta)G_{ji}+||b||^2cos^2(\theta)G_{jj} \tag{2.85}
bTGb=∣∣b∣∣2sin2(θ)Gii+2∣∣b∣∣2sin(θ)cos(θ)Gji+∣∣b∣∣2cos2(θ)Gjj(2.85)
利用正弦的倍角公式，
s
i
n
(
2
θ
)
=
2
s
i
n
(
θ
)
c
o
s
(
θ
)
sin(2\theta)=2sin(\theta)cos(\theta)
sin(2θ)=2sin(θ)cos(θ)，得到分子的最终形式为，
∣
∣
δ
x
h
∣
∣
2
=
∣
∣
b
∣
∣
2
[
s
i
n
2
(
θ
)
G
i
i
+
s
i
n
(
2
θ
)
G
i
j
+
c
o
s
2
(
θ
)
G
j
j
]
(2.86)
||\delta x_h ||^2=||b||^2[sin^2(\theta)G_{ii}+sin(2\theta)G_{ij}+cos^2(\theta)G_{jj}] \tag{2.86}
∣∣δxh∣∣2=∣∣b∣∣2[sin2(θ)Gii+sin(2θ)Gij+cos2(θ)Gjj](2.86)
利用
S
S
S的对称性，以类似的方式找到分母，
∣
∣
p
∣
∣
2
=
∣
∣
b
∣
∣
2
[
s
i
n
2
(
θ
)
S
i
i
+
s
i
n
(
2
θ
)
S
j
i
+
c
o
s
2
(
θ
)
S
j
j
]
(2.87)
||p||^2=||b||^2[sin^2(\theta)S_{ii}+sin(2\theta)S_{ji}+cos^2(\theta)S_{jj}] \tag{2.87}
∣∣p∣∣2=∣∣b∣∣2[sin2(θ)Sii+sin(2θ)Sji+cos2(θ)Sjj](2.87)
消去分子和分母的偏差项，得到如下表达式，
∣
∣
δ
x
∣
∣
∣
∣
p
∣
∣
=
[
s
i
n
2
(
θ
)
G
i
i
+
s
i
n
(
2
θ
)
G
j
i
+
c
o
s
2
(
θ
)
G
j
j
s
i
n
2
(
θ
)
S
i
i
+
s
i
n
(
2
θ
)
S
j
i
+
c
o
s
2
(
θ
)
S
j
j
]
1
2
(2.88)
\frac{||\delta x||}{||p||}=\Bigg[\frac{sin^2(\theta)G_{ii}+sin(2\theta)G_{ji}+cos^2(\theta)G_{jj}}{sin^2(\theta)S_{ii}+sin(2\theta)S_{ji}+cos^2(\theta)S_{jj}} \Bigg]^{\frac{1}{2}} \tag{2.88}
∣∣p∣∣∣∣δx∣∣=[sin2(θ)Sii+sin(2θ)Sji+cos2(θ)Sjjsin2(θ)Gii+sin(2θ)Gji+cos2(θ)Gjj]21(2.88)
使用毕达哥拉斯恒等式，这个表达式可以用正弦表示为，
∣
∣
δ
x
∣
∣
2
∣
∣
p
∣
∣
=
[
s
i
n
2
(
θ
)
(
G
i
i
−
G
j
j
)
+
s
i
n
(
2
θ
)
G
i
j
+
G
j
j
s
i
n
2
(
θ
)
(
S
i
i
−
S
j
j
)
+
s
i
n
(
2
θ
)
S
i
j
+
S
j
j
]
1
2
(2.89)
\frac{||\delta x||^2}{||p||}=\Bigg[ \frac{sin^2(\theta)(G_{ii}-G_{jj})+sin(2\theta)G_{ij}+G_{jj}}{sin^2(\theta)(S_{ii}-S_{jj})+sin(2\theta)S_{ij}+S_{jj}} \Bigg]^{\frac{1}{2}} \tag{2.89}
∣∣p∣∣∣∣δx∣∣2=[sin2(θ)(Sii−Sjj)+sin(2θ)Sij+Sjjsin2(θ)(Gii−Gjj)+sin(2θ)Gij+Gjj]21(2.89)
Larson展示了使用斜率法，即决策变量
D
=
p
T
p
D=p^Tp
D=pTp与水平位置误差的比例关系。这种方法在估计测量偏差对水平位置的影响，以设置检测阈值是有用的。在存在多个测量误差的情况下，这种方法也很有用，但随着误差的增加，精度会降低。进一步的分析可以在[16]中看到。
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）