状态估计基本

第二章 状态估计基础
状态估计的目的是对目标过去的状态进行平滑、对目标现在的运动状态进行滤波和对目标未来的运动状态进行预测。

这些运动状态包括目标位置、速度、加速度等。

本章讨论在多传感器跟踪系统中广泛应用的状态估计技术，这些技术包括Kalman 滤波技术，βα-滤波与γβα--滤波技术、最小二乘滤波技术和非线性系统的状态估计技术。

2.1 线性系统估计――卡尔曼滤波技术 2.1.1 线性系统描述
1.离散时间线性动态系统的状态方程
线性系统采用状态方程、观测方程及其初始条件来描述。

线性离散时间系统的一般状态方程可描述为
)()()()()1(k V k G k X k k X +Φ=+
其中，n R k X ∈)(是k 时刻目标的状态向量， n R k V ∈)(是过程噪声，它是具有均值为零、方差矩阵为)(k Q 的高斯噪声向量，即
[][
]
kl
T
k Q l V k V E k V E δ)()()(0
)(==
（δ：狄拉克函数，或单位冲激函数），n n R k ⨯∈Φ)(是状态转移矩阵，n n R k G ⨯∈)(是过程。

2. 传感器的测量（观测）方程 传感器的通用观测方程为 )()()()(k W k X k H k Z +=
这里，m R k Z ∈)(是传感器在k 时刻的观测向量，观测噪声m R k W ∈)(是具有零均值和正定协方差矩阵)(k R 的高斯分布测量噪声向量，即
[][
]
kl
T
k R k W k W E k W E δ)()()(0
)(==
3．初始状态的描述
初始状态)0(X 是高斯的，具有均值)0|0(ˆX
和协方差)0|0(P ，即 (
)(
){}
)0|0(0|0(ˆ)0(0|0(ˆ)0(P X X X X E T
=--
以上描述比较抽象，下面结合具体的例子加以说明。

例1：目标沿x 轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方程。

解：目标的状态为
⎥⎦
⎤
⎢⎣⎡=)()()(k x k x k X 用T 表示时间间隔，x u 表速度噪声，则有
⎩⎨
⎧+=+++=+)
()()1()()()()1(k u k x k x k Tu k x
T k x k x x x 写成矩阵形式为
)(1)()(101)1()1(k u T k x
k x T k x k x x ⎥⎦⎤
⎢⎣⎡+⎥⎦⎤⎢⎣⎡⨯⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡++ 令
)()(,1,101)(k u k V T G T k x =⎥⎦
⎤
⎢⎣⎡=⎥⎦⎤⎢⎣⎡=Φ 有
)()()()()1(k V k G k X k k X +Φ=+
其中
q k u E k V k V E k Q x T ===)]([)]()([)(2
)(k u x ：均值为0，方差为q 的高斯噪声。

例2：目标为二维空间中作匀速直线运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。

解：由于目标为二维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为
[]T k y
k y k x k x k y
k y k x k x k X )()
()()()()()()()( =⎥⎥⎥⎥
⎦⎤
⎢⎢⎢
⎢⎣⎡= 用T 表示时间间隔，y x u u ,分别表示y x ,方向的加速度噪声，则有
⎪⎪⎪⎩⎪⎪
⎪⎨⎧+=+++=++=+++=+)()()1()
(2)()()1()()()1()(2)()()1(2
2
k Tu k y
k y k u T k y T k y k y k Tu k x k x
k u T k x
T k x k x y y x x 写成矩阵的形式有
⎥⎦⎤⎢⎣⎡⨯⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎢⎢
⎣⎡+⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡++++)()(020002)()()()(10
00
10000
1000
1)1()1()1()1(22
k u k u T T T T k y
k y k x k x T T k y k y k x k x y x 令⎥⎥⎥⎥⎦⎤⎢⎢⎢
⎢⎣⎡=)()()()()(k y k y k x k x k X ，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=Φ10001000010001)(T T k ， ⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢
⎣⎡=T T T T k G 020002)(22
，⎥⎦
⎤⎢⎣⎡=)()()(k u k u k V y x 有
)()()()()1(k V k G k X k k X +Φ=+
[
][]
⎥⎥⎦⎤⎢
⎢⎣⎡=⎪⎭⎪⎬⎫⎪⎩⎪⎨⎧⎥⎦⎤⎢⎣⎡==))(())()(())()(())
(()()()()()()()(2
2
k u E k u k u E k u k u E k u E k u k u k u k u E k V k V E k Q y y x y x x y x y x T
假定)(),(k u k u y x 为均值为0，方差分别为x q 和y q 的相互独立的高斯白噪声，则
⎥⎦
⎤
⎢⎣⎡=⎥⎥⎦⎤⎢⎢⎣⎡=y x
y y x y x x q q k u E k u k u E k u k u E k u E k Q 0
0))(())()(())()(())(()(2
2
例3：目标为沿x 轴做匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程
解：目标的状态可写为：
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=)()()()(1k x k x k x k X 用T 表示时间间隔，z y x u u u ,,分别表示z y x ,,方向的加速度噪声，则有
⎪⎪⎪⎩
⎪
⎪⎪⎨⎧+=+++=++++=+)()()1()()()()1()(2)(2)()()1(22k u k x k x k Tu k x T k x k x k u T k x T k x
T k x k x x x x 写成矩阵的形式有
)(15.0)()()(100105.01)1()1()1(2
2k u T T k x k x k x T T T k x k x k x x ⨯⎥⎥⎥⎦
⎤
⎢⎢
⎢⎣⎡+⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⨯⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+++ 令
)()(,15.0)(,100105.01)(12121k u k V T T k G T T T k x =⎥⎥⎥
⎦
⎤
⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=Φ
有
)()()()()1(11111k V k G k X k k X +Φ=+
其中，
[]{}
x x T q k u E k V k V E k Q ===)()()()(2
1
x q 为x 方向加速度噪声的方差。

例4：在例3的基础上，假定目标为三维空间中作匀加速运动的目标，过程噪声为加速度噪声，试写出目标的状态方程。

解：由于目标为三维空间作匀速直线运动的目标，目标的状态中有目标的位置和目标的速度，那么目标的状态可写为
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=)()()()(321k X k X k X k X 其中
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=)()()()(,)()()()(,)()()()(321k z k z k z k X k y k y k y k X k x k x k x k X 目标的状态方程可写为
)()()()()1(k V k G k X k k X +Φ=+
其中
⎥⎥
⎥⎦
⎤
⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡ΦΦΦ=Φ)()()()(,)(000)(000)()(,)(000)(000)()(111111k u k u k u k V k G k G k G k G k k k k z y x 而过程噪声协方差矩阵为
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=z y x
q q q k Q 0
00
00)( z y x q q q ,,分别为x 方向，y 方向和z 方向加速度噪声方差。

例5：假定对二维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。

解：由前面可知，二维空间中作匀速直线运动的目标，其状态向量为
⎥⎥⎥⎥
⎦⎤⎢⎢⎢
⎢⎣⎡=)()()()()(k y
k y k x k x k X 由题意得目标的观测方程为
⎩⎨
⎧+=+=)
()()()
()()(k w k y k z k w k x k z y y x x 其中)(),(k w k w y x 分别为x 和y 方向的观测噪声。

将上式写成矩阵的形式，有
⎥⎦
⎤⎢⎣⎡+⎥⎥⎥⎥⎦
⎤
⎢⎢⎢⎢⎣⎡⎥
⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡)()()()()()(01000001)()(k w k w k y k y k x k x k z k z y x y x 令⎥⎦
⎤⎢⎣⎡=⎥⎦⎤
⎢⎣⎡=⎥⎦⎤⎢⎣⎡=)()()(,01000001)(,)()()(k w k w k W k H k z k z k Z y x y x ，则有 )()()()(k W k X k H k Z +=
[
][]
[]
[
]
[
]
[]
⎥
⎥⎦⎤
⎢⎢⎣
⎡=⎪⎭⎪⎬⎫⎪⎩⎪⎨⎧⎥⎦⎤⎢⎣⎡==)()()()()()
()()()()()()()(2
2
k w E k w k w E k w k w E k w E k w k w k w k w E k W k W E k R y y x y x x y x y x T
假定)(),(k w k w y x 为均值为零，方差分别为y x r r ,的高斯白噪声，则
[]
[
]
[
]
[]
⎥⎦
⎤
⎢⎣⎡=⎥⎥⎦⎤⎢⎢
⎣
⎡=y x
y y x y x x r r k w E k w k w E k w k w E k w E k R 0
0)()()()()()
()(2
2 例6：假定对三维空间作匀速直线运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。

解：由前面可知，三维空间中作匀速直线运动的目标，其状态向量为
⎥⎥⎥⎥⎥⎥
⎥⎥⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡=)()()()()()()(k z k z k y k y k x k x k X
由题意得目标的观测方程为
⎪
⎩⎪
⎨⎧+=+=+=)
()()()()()()()()(k w k z k z k w k y k z k w k x k z z z y y x x 其中)(),(),(k w k w k w z y x 分别为y x ,和z 方向的观测噪声。

将上式写成矩阵的形式，有
⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤⎢⎢⎢⎢⎢⎢⎢
⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡)()()()()()()()()(010000000100000001)()()(k w k w k w k z k z k y k y k x k x k z k z k z z y x z y x 有
)()()()(k W k X k H k Z +=
假定)(),(),(k w k w k w z y x 为均值为零，方差分别为z y x r r r ,,的高斯白噪声，则
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=z y x
r r r k R 0
00
00)( 作业：假定对三维空间作匀加速运动的目标进行观测时，观测值为目标的位置加上观测噪声，试写出目标的观测方程。

例7：设目标沿x 轴匀速直线运动，目标的状态可表示为[]T x
x
X =，在0t 时刻的x 观测值为)0(z ，在1t 时刻的x 观测值为)1(z ，采样间隔为01t t T -=，求目标的初始状态和初始协方差。

解：初始状态为
⎥
⎥⎦
⎤⎢⎢⎣⎡-=T z z z X
)0()1()1()1|1(ˆ 初始协方差
⎥⎦
⎤
⎢
⎣⎡=2/2//)1|1(T r T r T r r
P 其中，r 为观测噪声的方差，即：),0()(r N k W →,滤波器从2=k 时开始工作。

2.1.2 Kalman 滤波算法
状态估计的一步预测方程为
)|(ˆ)()|1(k k X
k k k X Φ=+ 一步预测的协方差为
)()()()()|()()|1(k G k Q k G k k k P k k k P T T +ΦΦ=+ 预测的观测向量为
)|1(ˆ)1()|1(ˆk k X k H k k Z
++=+ 观测向量的预测误差协方差为
)1()1()|1()1()1(+++++=+k R k H k k P k H k S T 新息或量测残差为
)|1(ˆ)1()1()|1(ˆ)1()1(k k X k H k Z k k Z
k Z k v ++-+=+-+=+ 滤波器增益为
)1()1()|1()1(1+++=+-k S k H k k P k K T Kalman 滤波算法的状态更新方程为
)1()1()|1(ˆ)1|1(ˆ++++=++k v k K k k X k k X
滤波误差协方差的更新方程为
[])
|1()1()1()
1()1()1()|1()1|1(k k P k H k K I k K k S k K k k P k k P T +++-=+++-+=++
2.2 转换坐标卡尔曼滤波器（非线性估计技术）。

卡尔曼滤波器两个要求： 1) 目标的状态方程是线性的； 2）观测方程是线性的。

在实际的情况下，要同时满足这两个要求是困难的。

通常情况下：状态方程在直角坐标系下是线性的，而观测方程是在极坐标系下获得的关于目标的测量。

如雷达的测量是距离、方位角和高低角。

从直角坐标系来看，观测方程是非线性的。

假定雷达的测量为距离r 、方位角β和高低角ε，测量与目标位置的关系为
⎪⎪⎪
⎪
⎩
⎪
⎪⎪⎪⎨⎧
+==++=2
2222arctan arctan y x z x y z y x r εβ
这是一个非线性关系。

为解决测量方程非线性情况下的目标跟踪问题，可采用两种方法，一种是采用扩展的卡尔曼跟踪滤波器，这将在后面介绍；另一种是对测量进行坐标转换，将极坐标下的测量转化为直角坐标系下的测量，然后用标准的卡尔曼滤波器进行跟踪。

对三坐标雷达，坐标转换公式为
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡−→−⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡z y x r r r r εβεβεεβsin sin cos cos cos 噪声方差的转换公式为
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤
⎢⎢⎢⎣⎡⨯⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=⎥⎥⎥
⎦⎤⎢⎢⎢⎣⎡⨯⎥⎥⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⇒⎥⎥⎥⎦⎤
⎢⎢⎢⎣⎡=εβεβεβεββε
εε
βεβ
εβεβεβεβεεβ
ε
βεβv v v A v v v r r r r r v v v z z r z
y y r y x x
r x
v v v V v v v V r r r z y x xyz r r cos 0
sin sin sin cos cos sin cos cos sin sin cos cos cos
()
{
}
[]T A v v v v v v E A V V E R r
r T
xyz xyz xyz ⨯⎪⎭
⎪
⎬⎫⎪⎩⎪⎨⎧⎥⎥⎥
⎦
⎤⎢⎢⎢⎣⎡⨯==εβ
εβ 假设距离、方位角和高低角的测量噪声为相互独立的高斯白噪声，即 0)()()()()()(======βεεβεεββv v E v v E v v E v v E v v E v v E r r r r 并且
()()()
222
222,,εεββσσσ===v E v E v E r r T r z zy zx yz y yx xz xy x xyz
A A R ⨯⎥⎥⎥⎦⎤⎢⎢⎢⎣
⎡⨯=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=22
22222222220
00
000
εβ
σσσσσσσσσσσσ 对两坐标雷达，极坐标系下的观测为（θ,r ），转换成直角坐标为
⎩⎨⎧==β
βsin cos r y r x
方差的转换：为求方差的转换，先求偏导
⎥⎦
⎤
⎢⎣⎡=⎥⎦⎤
⎢⎣⎡⎥⎦⎤⎢⎣
⎡-=⎥⎦⎤⎢⎣⎡ββββββd dr A d dr r r dy dx cos sin sin cos [][]T T
A d dr d dr A d dr A d dr A dy dx dy dx ββββ⎥⎦
⎤
⎢⎣⎡=⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡⨯⎥⎦⎤⎢⎣⎡=⎥⎦
⎤
⎢⎣⎡ 于是，得
T
r y xy xy
x xy A A R ⎥
⎥⎦⎤⎢⎢⎣⎡=⎥⎥
⎦
⎤⎢⎢⎣⎡=22
222
200βσσσσσσ
()
⎪⎪⎩
⎪⎪⎨⎧-==+=+=2222222
2222222222cos sin cos sin sin cos ββββσσββσσβ
σβσσβσβσσr r r yx xy r y r x 在得到直角坐标系下的测量及测量的协方差矩阵后，可以用前面讲的方法进行目标跟踪。

这种变换存在的问题是：在直角坐标系下，测量的噪声不再是严格意义上的高斯噪声，并且噪声的分布中心并不是以直角坐标),,(z y x 为中心，而且在直角坐标系下噪声是相互关联的。

[]x r x E ≠β, []y r y E ≠β,
由于噪声的分布中心并不是以直角坐标),,(z y x 为中心，因此，噪声是有偏的，为此，有些学者提出了无偏坐标转换的方法：对转换后的直角坐标和方差进行校正。

通常情况下，传感器的测量为极坐标测量，极角坐标测量z 可通过无偏坐标转换变为直角坐标测量。

设[]T r
β=z ，则无偏转换的测量c z 为
⎥⎥⎦⎤⎢
⎢⎣⎡=⎥⎥⎦⎤⎢⎢⎣⎡=--
βλβλββsin cos 11
r r y x c c c
z
其中，2
/2
βσβλ-=e。

转换测量的协方差为
⎥⎦
⎤⎢⎣⎡=c c
c c
c
R R R R 222112
11
R 其中
)2cos 1)((21cos )2(4222
2211βλσβλββ+++-=-r c r r R
)2cos 1)((2
1sin )2(4222
2222βλσβλββ-++-=-r c r r R
βλσββλββ2sin )(2
1sin cos )2(4222
22112r c c r r R R ++-==-
在动态方程为线性、观测方程为非线性的情况下，可使用转换坐标卡尔曼滤波器，转换可采用两种方法：一种是不带校正量的转换，这是一种相对简单的方法；另一种为带校正量的方法，这是一种相对复杂的方法。

2.3 扩展卡尔曼滤波器(非线性估计技术) 采用扩展的卡尔曼滤波器进行目标跟踪。

2.
3.1 系统的状态方程和测量方程 状态方程：
)()()()()1(k W k G k X k k X +Φ=+
其中
kj
T k Q j W k W E k W E δ)()()([0
)]([==
)(k Φ为状态转移矩阵。

测量方程：
[])1()1()1(+++=+k V k X h k Z 其中
kj
T
k R j V k V E k V E δ)()()([0
)]([==
2.3.2、观测方程线性化
将等号后的第一项以)|1(ˆk k X
+为中心按泰勒级数展开并略去二级以上的高阶分量，有
[]
)|1(ˆ)1()]|1(ˆ[)]1([)|1(ˆk k X
k X X
h k k X
h k X h k k X
X +-+⨯∂∂++=++=
代入上式，得
[]
)1()|1(ˆ)1()]|1(ˆ[)1()|1(ˆ+++-+⨯∂∂++=++=k V k k X
k X X h k k X
h k Z k k X
X []
)1()|1(ˆ)1()]|1(ˆ[)1()1|(ˆ+++-+⨯∂∂=+-+-=k V k k X
k X X h k k X
h k Z k k X
X 令
)|1(ˆ)1(k k X
X X
h
k H +=∂∂=
+，
)]|1(ˆ[)1()1(~k k X
h k Z k Z +-+=+，
)|1(ˆ)1()|1(~k k X
k X k k X +-+=+，于是得到线性化的观测方程为 )1()|1(~
)1()1(~++++=+k V k k X k H k Z
假定目标的状态为T 1],,[n x x X =,而[]T
1)(,),()(X f X f X h m =，则
)|1(ˆ111
1)()()()
()1(k k X X n m m n x X f x X f x X f x X f k H +=⎥⎥⎥
⎥
⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂∂∂=+ 为)(X h 的雅可比矩阵(Jacobian 矩阵)。

例：雷达的观测是在极坐标系下进行的，对于一个直角坐标为),,(z y x 的目标，雷达所测的三个极坐标分别为
⎪⎪⎪
⎪
⎩
⎪
⎪⎪⎪⎨⎧
+====++==2
2322221arctan ),,(arctan
),,(),,(y x z z y x f x y z y x f z y x z y x f r εβ 观测方程为
)1()]1([)1(arctan arctan
)1(2
2
2
22+++=++⎥⎥⎥⎥⎥⎥
⎥⎦
⎤
⎢⎢⎢⎢⎢⎢
⎢⎣⎡+++=+k V k X h k V y x z x y
z y x k Z ⎥⎥⎥⎥⎥⎥⎥⎦
⎤
⎢⎢⎢⎢⎢⎢
⎢⎣
⎡
+++=2
2
2
22arctan arctan
)(y x z x y
z y x X h 令目标的状态为T z
z z y
y
y x
x x
X ][ =，则线性化的观测矩阵为 )|1(ˆ2
222
22
2222222)|1(ˆ333333333222222222
111
1
11
1
1
1
000
00
00000000000000)1(k k X
X k k X X r y x y
x r
yz y x r xz y x x y x y r z r y r x z f z
f z f y f y f y f x f x
f x
f z f z f z f y f y f y f x f x f x f
z f z f z f y f y f y f x f x f x f k H +=+=⎥
⎥
⎥⎥⎥
⎥⎥
⎥⎦⎤
⎢⎢
⎢
⎢⎢
⎢⎢⎢⎣
⎡++-+-++-=⎥⎥⎥⎥⎥⎥⎥⎦⎤
⎢⎢⎢⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂=+ 其中222z y x r ++=
例：红外传感器,观测值为方位角β和高低角ε ，求目标的状态为
[]T z
z
y
y
x
x
X =时线性化的观测矩阵 解：因⎥⎥⎥⎥⎦
⎤
⎢⎢⎢⎢⎣⎡+=22arctan arctan )(y x z x
y X h
)|1(ˆ2
222
22
2222222)|1(ˆ00
0000)1(k k X
X k k X
X r y x y
x r
yz y x r xz y x x
y x y X
h k H +=+=⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣
⎡
++-+-++-=∂∂=
+2.3.3、扩展卡尔曼滤波方程
一步预测：
)|(ˆ)()|1(ˆk k X k k k X
Φ=+ 一步预测协方差矩阵
)()()()()()()|1(k G k Q k G k k P k k k P T
T
+ΦΦ=+
预测的观测向量为
)]|1(ˆ[)|1(ˆk k X h k k Z
+=+ 观测矩阵 )|1(ˆ)1(k k X
X X
h
k H +=∂∂=
+
新息或量测残差为
)]|1(ˆ[)1()|1(ˆ)1()1(k k X h k Z k k Z
k Z k v +-+=+-+=+ 残差协方差矩阵
)1()1()|1()1()1(+++++=+k R k H k k P k H k S T
滤波器增益
)1()1()|1()1(1
+++=+-k S k H k k P k K T
滤波输出
)]}|1(ˆ[)1(){1()|1()1|1(ˆk k X h k Z k K k k X k k X
+-++++=++ 误差的协方差阵
)|1()]1()1([)1|1(k k P k H k K I k k P +++-=++
例题2-2：利用扩展的卡尔曼滤波算法对y 轴上匀速运动的目标进行跟踪。

设传感器平台在x-y 平面内运动，运动方程为
⎩⎨
⎧+=+-=s s s s dy t y dx x 450000
式中，t 为时间，s dx 和s dy 为相互独立的、零均值白色高斯噪声，其方差分别为221,1m r m r y x ==，
且与过程噪声和量测噪声相互独立。

过程噪声为加速度噪声，其方差为()
2
22/16s m v =σ，传感器测量目标的方位角，采样时间间隔s T 2=，测
量噪声为零均值的高斯噪声，其方差为2
2
18014159.33)3(⎪⎭
⎫
⎝⎛⨯==rad r 。

解：目标在y 轴上运动，系统的状态方程为
)()()()()1(k v k G k X k k X +Φ=+
其中，[]'=)()
()(k y
k y k X ⎥⎥
⎦
⎤⎢⎢⎣⎡=⎥
⎦⎤
⎢⎣⎡=ΦT T k G T k 2)(101)(2
传感器的测量方程为
)()]([)(k w k X h k z +=
其中， ⎪⎪⎭
⎫ ⎝⎛--=⎪⎪⎭⎫ ⎝⎛--=•s s s s
x
y y x
x y y h arctan arctan )( 建立目标位置估计值)(ˆk y
和方位角测量值)(k z 之间的关系(不考虑测量误差)为 ⎪⎪⎭
⎫
⎝
⎛--=s s x
y k y
k z )(ˆarctan )( 所以
))(tan()()()(ˆk z k x k y k y
s s -=, 由 ),1(ˆ),0(ˆ),1(),0(y y z z →
目标的初始状态
⎥
⎥⎦
⎤⎢⎢⎣⎡
-=T y y y )0(ˆ)1(ˆ)1(ˆ)1|1(ˆX
对上式求导可得
dz k z x k z dx dy y
d s
s s ))
((cos ))(tan(ˆ2
--= 于是
r k z x
k z r r k z x k z s
x y z s x y k y
s
s
))
((cos ))((tan ))
((cos ))((tan 4
222
4
2
2
22
2)(ˆ+
+=++=σσσ
σ
初始状态的方差
⎥⎥⎦
⎤
⎢⎢⎣⎡=22)1(ˆ2)1(ˆ2)1(ˆ2
)
1(ˆ
/2//)1|1(T T T P y y
y y σσσσ 线性化的观测矩阵
)|1(ˆ2
2
)|1(ˆ0)
(][]
[)1()1(k k X
X s s s k k X
X X y y x x y X h y X h k h k H +=+=⎥
⎦⎤⎢⎣⎡-+-=⎥⎦⎤
⎢
⎣⎡∂∂∂∂=+=+
2.4 无迹卡尔曼滤波器（Unscented Kalman Filter, UKF ）
目前，扩展卡尔曼滤波虽然被广泛用于解决非线性系统状态估计问题，但其滤波效果在很多复杂的系统中并不能令人满意。

模型的线性化误差往往会严重影响最终的滤波精度，甚至导致滤波器发散。

另外，在许多实际的应用中，模型的线性化过程比较繁杂，而且也不易得到。

无迹卡尔曼滤波是由于不需要对非线性系统进行线性化，并可以很容易地应用于非线性系统的状态估计，因此，UKF 方法在许多方面得到应用。

2.4.1 无迹变换
无迹卡尔曼滤波是在无迹变换的基础上发展起来的。

无迹变换（Unscented Transformation, UT ）的基本思想是由Julier 等首先提出的，是用于计算经过非线性变换的随机变量统计的一种新方法。

它不需要对非线性状态和测量方程模型进行线性化。

假定X 为一个X n 维的随机变量，y
X n n R
R g →:为一非线性函数，并且
)(X g Y =。

X 的均值和协方差分别为X 和X P 。

无迹变换的一般步骤如下：
（1） 确定(12+X n )个δ采样点
(
)
(
)
X
i
X
X n i X i
X
X i n i P n X X n i P n X X i X X X ,,1)(-
,,1)(0
;0 =+==++
===+λλ X
X
n i X X i X n i n W n i n W i n W X ,,1)
(21,,1)
(210;
)(0 =+==+==+=+λλλλ 其中，λ为一尺度参数，可以为0≠+λn 的任意值。

()
i X X P n )(λ+为X X P n )(λ+的第i 列。

X n 为状态向量X 的维数。

（2） 每个δ采样点通过非线性函数传播，得到
)(i i X g Y =； X n i 2,,1,0 =
（3） Y 的估计值和协方差估计如下 ∑==X
n i i i Y W Y 20
Y Y Y Y W P i i n i i Y X
T 20
)-)(-(∑==
2.4.2 无迹Kalman 滤波算法
根据无迹变换的特点，Kalman 滤波算法可归纳如下：
假定
状态方程：)()()]([)1(k V k G k X f k X +=+ 观测方程：)()]([)1(k W k X h k Z +=+
k 时刻的状态估计值和状态协方差：)|(ˆk k X
和)|(k k P k 时刻的观测向量)1(+k Z
（1） 根据采样规则，确定2X n +1个采样点以及相应的加权值
(
)
(
)
X
i
X n i X i
X i
n i k
k P n k k X X n i k
k P n k k X
X i k k X
X X ,,1|()(-
)|(,,1|()()|(ˆ0
;)|(ˆ0 =+==++===+λλ X
X n i X X i X n i n W n i n W i n W X ,,1)
(21
,,1)(21
0;
)
(0 =+==+==+=+λλλλ 其中，λ为尺度参数，可以为0≠+λX n 的任意值。

()
i
X k k P n )1-|1-()(λ+为)1-|1-()(k k P n X λ+均方根的第i 列。

X n 为状态向量X 的维数。

（2） 一步预测
][)|1(i i X f k k X =+； X n i 2,,0 =
)|1()|1(20
k k X W k k X i n i i x
+=+∑=
{}{}
T
i i n i i T
XX k k X k k X k k X k k X W k G k Q k G k k P X
)|1()|1()|1()|1()()()()|1(20
+-+⨯+-++=+∑=)]|1([)|1(k k X h k k Z i i +=+； X n i 2,,0 =
)|1()|1(20
k k Z W k k Z i n i i X
+=+∑=
{}{}
T
i i n i i ZZ k k Z k k Z k k Z k k Z W k R k k P X
)|1()|1()|1()|1()1()|1(20
+-+⨯+-+++=+∑={}{}T
i i n i i XZ k k Z k k Z k k X k k X W k k P X
)|1()|1()|1()|1()|1(20
+-+⨯+-+=+∑=
（3） 更新状态
)
1()|1()1()|1()1|1())|1()1()(1()|1()1|1(ˆ)
|1()|1()1(1
+++-+=+++-++++=++++=+-k K k k P k K k k P k k P k k Z k Z k K k k X k k X
k k P k k P k K T ZZ XX ZZ XZ 在动态方程和观测方程同时为非线性、或者一个为线性另一个为非线性的情况下，可以使用无迹卡尔滤波器，无迹卡尔曼滤波器的缺点是计算量较大。

与无迹卡尔滤波器类似的滤波器还有积分卡尔滤波器（QKF ）和粒子滤波器，这些滤波器均可用于非线性系统，其缺点是计算量更大。

2.5 βα-和γβα--滤波器
βα-和γβα--滤波器分别用于对匀速和匀加速目标进行跟踪。

2.5.1、βα-滤波器
βα-滤波器用于对匀速运动的目标进行跟踪。

目标的状态方程为
)()()1(k V k X k G ΦX +=+
式中，
⎥⎦⎤⎢⎣⎡=x x k X )(，⎥⎦⎤⎢⎣⎡=101T Φ，⎥⎦
⎤⎢⎣⎡=T T 2G ，[]kj V j k δσ2
)()(='V V E 目标的观测方程
)()()(k k k W HX z += 式中，
[]01=H ，[]kj W j k δσ2
)()(='W W E
滤波方程
[][])|1(ˆ)1()|1(ˆ)|1(ˆ)1()|1(ˆ)1|1(k k k T k k k k k k k k k +-+⎥
⎦
⎤⎢⎣⎡++=+-+++=++z z X z z K X
X βα
问题：如何求α和β，利用稳态滤波器的特点，即 )1|1(lim )|(lim ++=∞
→∞
→k k P k k P k k
⎥⎦
⎤
⎢
⎣⎡≡∞
→T k K k /)(lim βα 得
8
8)4(822λ
λλλλα++-+-=
4
8422λ
λλλλβ+-+=
其中，22
42
W
V
T σ
σλ=
，λ为目标的机动系数。

状态估计
[])|1(ˆ)1()|1(ˆ)1|1(k k x k k k k k +-+⎥⎦
⎤⎢⎣⎡++=++z X X βα 状态估计的协方差为
⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡--=22
2
2
2
)1(2/)(W W W W
T T T
σαβαββσβσασP
一步预测协方差为
⎥⎥⎥⎥⎦
⎤
⎢⎢⎢
⎢⎣⎡+----==+∞→22
2222
22)1()
1()
1(1)|1(lim W
W W
W
W k T T T T M k k P σσααβσαβσαβ
σα
α 以上只讨论的是沿x 轴作直线运动情况下的βα-滤波，对于平面或空间目标，需要分别对x 、y 及z 上的状态向量分别βα-滤波。

当过程噪声方差不能事先确定时，目标的机动系数无法确定，α和β两参数无法确定。

工程上经常采用两种的方法来确定α和β： 1、常系数法：α和β取固定的值 一般取5.0~3.0=α
(1) ααβ---=122 (临界阻尼法)
(2) α
αβ-=12
(最佳选择法)
2、变系数法：
⎪⎪⎩
⎪⎪⎨
⎧+=
+-=)1(6)
1()12(2k k k k k βα 这里k 从1开始计数。

对β来说2=k 时才有值，但滤波器从3=k 开始工作，前两个点用于确定目标的初始位置和速度，完成航迹起始。

2.5.2 γβα--滤波器
γβα--滤波器用于对匀加速运动的目标进行跟踪。

目标的状态方程为
)()()1(k V k X k G ΦX +=+
式中，
⎥⎥
⎥⎦⎤
⎢⎢⎢⎣⎡=x x x k X )(，⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=10010212T T T Φ，⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡=122T T G ，[]kj V j k δσ2
)()(='V V E 目标的观测方程
)()()(k k k W HX z += 式中，
[]001=H ，[]kj W j k δσ2
)()(='W W E
滤波方程
[][])|1(ˆ)1()|1(ˆ)|1(ˆ)1()|1(ˆ)1|1(2
k k k T k k k k k k k k k +-+⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡++=+-+++=++z
z X z z K X
X γβα 问题：如何求α、β和γ，利用稳态滤波器的特点，即
)1|1(lim )|(lim ++=∞
→∞
→k k P k k P k k
⎥⎥⎦
⎤⎢⎢⎣⎡≡∞→2//)(lim T T k K k γβα 得三个非线性方程构成的方程组
⎪⎪⎪⎩
⎪
⎪⎪
⎨⎧=---==-αβγααβλαγ2
22
14)2(2)1(4
其中，2242
W
V
T σ
σλ=
，λ为目标的机动系数。

解线性方程组，得α、β和γ。

状态估计
[])|1(ˆ)1()|1(ˆ)1|1(2
k k x k T T k k k k +-+⎥⎥
⎥⎦
⎤
⎢⎢⎢⎣⎡++=++z X X γβα 状态估计的协方差为
⎥⎥⎥
⎥⎥
⎥⎥⎦
⎤
⎢⎢⎢
⎢⎢⎢⎢⎣
⎡
---------+=24
2
322232
2
2
222
2
)1(4)2()
1(4)2()
1(4)2()1(8)42(8W W
W W W
W W W W T T T T T T
T
σαγβγσαγββγσσαγββσααβγαββσγσβσασP 以上只讨论的是沿x 轴作直线运动情况下的γβα--滤波，对于平面或空间目标，需要分别对x 、y 及z 上的状态向量分别γβα--滤波。

当过程噪声方差不能事先确定时，目标的机动系数无法确定，α、β和γ三参数无法确定。

工程上经常采用两种的方法来确定α、β和γ 1、常系数法：α和β取固定的值 一般取5.0~3.0=α
⎪⎪⎩
⎪⎪⎨⎧--=-+-+-=
αααβγααααααβ2
4
5623)2()
1(864644)42( 2、变系数法：
⎪
⎪⎪⎩
⎪⎪⎪⎨⎧++=++-=+++-=
)2)(1(60)2)(1()
12(8)2)(1()233(32k k k k k k k k k k k k γβα
这里k 从1开始计数。

对β来说2=k 时才有值，对于γ来说3=k 时才
有值，但滤波器从4 k开始工作，前三个点用于确定目标的初始位置、速度和加速度，完成航迹起始。

2.6 最小二乘估计和最小二乘滤波器 2.6.1、线性最小二乘估计 模型：
i i i V X C Y += m i ,,2,1 = V CX Y +=
其中， ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=m Y Y Y Y 2
1；⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=m C C C C 21；⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡=m V V V V 21
目标：求X ˆ以使m in )()(=--=CX Y CX Y V V T T 方法：0)
(=∂∂X
V V T 得到估计：Y C C C X
T T 1)(ˆ-= 估计误差的方差：11)()(--=C C RC C C C P T T T 其中0)()(>==T VV E V Var R
例：设目标作匀速直线运动，目标的状态可表示为
[])()
()()
(t y
t y t x t x ，目标的起始状态为[])()
()()(0000t y
t y t x t x ，目标的起始时刻为0t ，在1t 时刻的观测值为),(11y x ，在2t 时刻的观测值为
),(22y x ，观测噪声为相互独立的高斯白噪声，m y x 10==σσ，分别求0
t 和2t 时刻目标状态的最小二乘估计。

a 、求0t 时刻目标状态的估计值
解：⎥⎥⎥
⎥⎥⎦⎤
⎢⎢⎢⎢⎢⎣⎡+⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎦⎤⎢
⎢⎢
⎢⎣⎡----=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡221
100000202010
12211)()()()(100
0011000
1y x y x v v v v t y t y t x t x t t t t t t t t y x y x
V CX Y +=
得到估计：Y C C C t X T T 10
)()(ˆ-= 估计误差的方差：110)()()(--=C C RC C C C t P T T T 其中
()
()100100100100)()(2
222diag diag VV E V Var R y x y x T ====σσσσ
b 、求2t 时刻目标状态的估计值
⎥⎥⎥
⎥⎥⎦⎤
⎢⎢⎢⎢⎢⎣⎡+⎥⎥⎥⎥⎦⎤⎢
⎢⎢
⎢⎣⎡⎥⎥⎥⎥⎦⎤⎢
⎢⎢⎢⎣⎡--=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡221
12222212
12211)()()()(0100
000110000
1y x y x v v v v t y t y t x t x t t t t y x y x V CX Y
+=
得到估计：Y C C C t X T T 12
)()(ˆ-= 估计误差的方差：112)()()(--=C C RC C C C t P T T T 其中
()
()100100100100)()(2
222diag diag VV E V Var R y x y x T ====σσσσ
2.6.2、加权最小二乘估计
当测量数据的测量精度不等时，应采用加权处理，对精度较高的测量结果赋以较大的权。

目标：求X
ˆ以使min )()(1
1
=--=∑∑==X C Y X C Y W V V i i T i i k
i i i k
i T
i 得到估计：WY C WC C X
T T 1)(ˆ-= 估计误差的方差：11)()(--=WC C WRWC C WC C P T T T 其中0)()(>==T VV E V Var R 2.6.3、马尔可夫估计
马尔可夫估计是一切加权最小二乘估计中能使得估计方差阵达到
最小的一种。

当取1-=R W 时，加权最小二乘估计为马尔可夫估计
估计值为：Y R C C R C X
T T 111)(ˆ---= 其估计误差的方差为
11)(--=C R C P T
2.6.4、非线性最小二乘估计 模型：
i i i V X f Y +=)( m i ,,2,1 = V X F Y +=)(
目标：求X
ˆ以使min ))(())((1
1
=--=∑∑==X f Y X f Y V V i i m
i T i i i m
i T
i 另一种表示
⎪⎭
⎫ ⎝⎛--=∑=))(())((min arg ˆ1X f Y X f Y X i i m i T i i X
加权最小二乘估计
目标：min ))(())((1
1
=--=∑∑==X f Y X f Y c V V c i i m
i T i i i i m
i T
i i
令i
i i i
i i c X f X f c Y Y )()(,11=
=
，非线性加权最小二乘简化为非线性最小二乘
min ))(())((111
11=--∑=X f Y X f Y
i i m
i T i i。

非线性最小二乘估计可以用Gauss-Newton 算法或Levenberg-Marquart 算法（这些算法在Matlab 中都已编好，直接应用就行，如lsqnonlin ，当然也可自行编写）求解。

这里给出一种
自我实现的方法。

令
⎥⎥
⎥⎦⎤
⎢⎢⎢⎣⎡--=)()()(11X f Y X f Y X m m p E T
)()(X X d X p ∂=
E J
⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢
⎢⎣
⎡∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂∂-=-==z f z
f y f y f x f x f z f z f y f y f x f x
f dX X d dX X d X m m m m m m p 11
11
11
T
T )()()(F E J 则Levenberg-Marquart 算法求解非线性最小二乘的迭代公式为
[]
)()()()(11T 1
111T 1-------+-=k p k k k k k k X X u X X X X E J I J J
令
)()(T X X J J A =
一种伪语言描述的LM 算法如下：
310-=τ;e1:=10-15;e2:=10-15; e3:=10-15;kmax=100;
k:=0;v:=2;p=X 0;
A:=J T *J;Ep:=Y-F(p);g:=J T *Ep; Stop:=(1e g
≤∞
);
)(max *,,1ii m i A ==τμ;
while(not stop)and(k<kmax)
k:=k+1;
repeat
Solve (A+μI)*p δ=g
if (p e p *2≤δ) stop:=true;
else pnew:=p+p δ;
(
)));(/()
(:2
2g pnew Ep p T p
+--=μδδ
ρF Y
if 0>ρ
p:=pnew;
A:=J T *J;Ep:=Y-F(p);g=:J T *Ep;
Stop:=(1e g
≤∞
) or 32
e Ep ≤;
⎪⎭
⎫ ⎝⎛--=3)12(1,31max *:ρμμ;v:=2;
else
;
*2:;
*:v v v ==μμ
endif
endif
until(0>ρ)or(stop)
endwhile X:=p;
非线性最小二乘估计也是一种最大似然估计，是一种最优估计，估计误差的方差能达到CRLB ，因此估计误差的方差可取为CRLB 。

估计误差的方差为
[]
1ˆ)
()(-=≈X
X T X J X J P
例：匀速运动目标，目标的状态为[]T t y t y t x
t x t X )()()()()( =，1t 时刻的测量为[]T r 11θ，2t 时刻的测量为[]T r 22θ，传感器测距误差标准差为r σ=10m,测角误差标准差为θσ=0.001rad, 求2t 时刻目标的状态。

设2t 时刻为[]T t y t y t x t x t X )()()()()(22222 =，则1t 时刻目标的状态1X 为
⎥⎥
⎥⎥⎦⎤
⎢
⎢⎢
⎢⎣⎡⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡)()()()(1000100001000
1)()()()(2222212
11111t y t y t x t x t t t t t y
t y t x t x 1t 时刻的理论观测值为 ()()[]()()[]2221222212121221)()()()()()())((t y t t t y t x
t t t x t y t x t X f r -++-+=
+= ()()()⎪⎪⎭
⎫ ⎝⎛-+-+=⎪⎪⎭⎫ ⎝⎛=)()()()(arctan )()(arctan ))((221222121121t x t t t x t y
t t t y t x t y t X f θ 2t 时刻的理论观测值为
)()())((222222t y t x t X f r +=
⎪⎪⎭
⎫
⎝⎛=)()(arctan ))((2222t x t y t X f θ
加权非线性最小二乘估计的目标函数为
()()()()⎥
⎦
⎤
⎢⎣⎡-+-+-+-=222222222222112221122))((1))((1))((1))((1))((t X f t X f r t X f t X f r t X B r r r r θθθθθσσθσσ2t 时刻目标状态2X 的估计为
()))((min arg ˆ2
22
t X B X X = Matlab 求Jacobian 矩阵的方法
syms x xv y yv dt std_r std_b r1 b1 r2 b2 y1=(r1-sqrt((x+dt*xv)^2+(y+dt*yv)^2))/std_r; y2=(b1-atan((y+dt*yv)/(x+dt*xv)))/std_b;
y3=(r2-sqrt(x^2+y^2))/std_r; y4=(b2-atan(y/x))/std_b;
J=jacobian([y1;y2;y3;y4],[x xv y yv]); J=simplify(J)
例2-3 目标为二维空间中作匀速直线运动的目标，传感器为2维雷达，测量目标的距离和方位角，传感器位置为T ]0,1000[m m ，其测距误差标准差为100 m,测角误差标准差为0.001弧度。

在s t 00=时，观测值为[49999.7, 1.591196]，在s t 101=时，观测值为[51111.3, 1.561589]，在s t 202=时，观测值为[52043.8, 1.532488]，分为用线性最小二乘和非线性最小二乘法估计目标的初始状态和状态估计的协方差。

2.6.5、最大似然和非线性最小二乘的转化
例：匀速运动目标，目标的状态为[]T y y x
x X =，1t 时刻的测量为[]T r 11θ，2t 时刻的测量为[]T
r 22θ，传感器测距误差标准差为r σ=10m,测角误差标准差为θσ=0.005rad, 求2t 时刻目标的状态的最大似然估计。

正态分布概率密度函数
2
2121
)(⎪⎭
⎫ ⎝⎛--=
σμπ
σx e
x p
假定观测为相互独立的0均值高斯白噪声。

测量为[]T r 11θ的概率密度函数为
2
)(212
)(21121121121
21
⎪⎪⎭
⎫
⎝⎛--⎪⎪⎭
⎫
⎝⎛--•
=
Λθθσθθσπ
σπ
σX f X f r r e
e
r r
[]T r 22
θ的概率密度函数为
2
)(212
)(21222222221
21
⎪⎪⎭
⎫
⎝⎛--⎪⎪⎭
⎫
⎝⎛--•
=
Λθθσθθσπ
σπ
σX f X f r r e
e
r r
测量为[]T r 11θ和测量[]T r 22θ同时出现的联合概率密度函数（似然函数）为
()()()())2(2
1
222222222222112221122
22)(1
)(1)(1)(121222214141X J r r r r e
e
r X f X f r X f X f r r -=
=Λ•Λ=Λ⎥⎥⎦
⎤⎢⎢⎣⎡-+-+-+--θθσσθσσθσσπσσπθθθθ
对数似然
)(2141
ln ln 2222X J r -⎪⎪⎭⎫ ⎝
⎛=Λθσσπ
))((min arg )(ln max arg ˆ2
2X J X X
X
=Λ=
2.6.6、最小二乘滤波器 模型： 状态方程
)()1(k X k X Φ=+
观测方程
k k k V k X C Y +=)( m k ,,2,1 =
求)(ˆm X
按最小二乘估计的方法，先确定m Y Y ,,1 与)(m X 间的关系
)()1(1m X m X -Φ=-
)()()1()2(211m X m X m X --Φ=-Φ=-
)()()1(11m X X m --Φ=
得到最小二乘估计
Y C m P Y C C C m X
T T T )()()(ˆ1==- 其中， ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=m Y Y Y Y 21；⎥⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎢⎣⎡ΦΦ=----m m m C C C C )2(2)1(1；⎥
⎥⎥⎥⎦⎤
⎢⎢⎢⎢⎣⎡=m V V V V 21；1)()(-=C C m P T 取加权矩阵，作用是使过老数据的作用逐渐消失。

取0>β
⎥⎥⎥⎥
⎥⎦⎤⎢⎢⎢⎢
⎢
⎣⎡=----100
000000
00
0)2()1( β
β
m m e e W 则加权最小二乘估计为
WY C m P WY C WC C m X
T T T )()()(ˆ1==-
其中
1)()(-=WC C m P T
现在假设知道 m
Y m P m X ),1(),1(ˆ--，求)(ˆm X ，于是就形成了最小二乘滤波器。

加权最小二乘滤波器的递推公式为
)]1(ˆ[)()1(ˆ)(ˆ--+-=m m m m m
m T m X ΦC Y C P X ΦX D D I D D m P m T m m T m C C C C 1
)()(-+-=
其中，βe m P D T ΦΦ)1(-=。

例：设目标为匀速运动模型，即状态向量为
[]T t z
t z t y t y t x t x t )()
()()()()
()( =X 观测为目标在直角坐标下的位置，写出最小二乘滤波器中的状态转移矩阵Φ及观测矩阵k C 。

解：转移矩阵为
;000000⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=φφφΦ 其中⎥⎦⎤⎢⎣⎡-=⎥⎦⎤⎢⎣⎡∆=-1011011k k t t t φ 观测矩阵为
⎥⎥
⎥⎦
⎤
⎢⎢⎢⎣⎡=010000000100000001k C
k 时刻的观测
⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=)()()()(k z k z k z k Y z y x 例2-4 目标为二维空间中作匀速直线运动的目标，其初始状态为
[]T
0/150,50000,/150,0s m m s m m X =, 过程噪声的方差q=1。

传感器为2
维雷达，测量目标的距离和方位角，传感器位置为T ]0,1000[m m ，其测距误差标准差为100 m,测角误差标准差为0.001弧度，采样时间间隔为10 s 。

试：
1、根据已知条件产生仿真的观测数据。

2、利用仿真的观测数据分别用变系数βα-滤波器、转换坐标Kalman 滤波器、最小二乘滤波器和扩展Kalman 滤波器进行目标跟踪，并在同一图上绘制出跟踪结果。

2.7 空间配准和坐标变换
在对坐标位置的不同传感器所获得的数据进行处理时，首先要作的是将测量数据转换到公共的坐标系下，然后才能对数据进行处理。

通常的坐标系有以下4种：
（1） 以本传感器位置为中心的直角坐标系（传感器局部坐标系） （2） 以地球中心为中心的直角坐标系（地心坐标系） （3） 以经度、纬度、高度表示的大地坐标系（地理坐标系） 假设在局部坐标系中
x :正东为正,
y :正北为正, z:向上为正；
方位角β：正东为0，逆时针为正， 高低角ε：水平为0，逆时针为正。

主要坐标变换有：
（1）地理坐标()h ,,φλ到地球固定坐标的转换关系为
()[]
⎥⎥
⎥⎦
⎤
⎢⎢⎢⎣⎡+-++=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=φρλφλφφλsin 1sin cos )(cos cos )(),,(2
h N h N h N Z Y X h P 其中，h ,,φλ分别表示地理的经度、纬度和高度，km R e 137.6378=为地球的长轴半径，km r e 7523142.6356=为地球短轴半径，
0818192.0122=-=e e R r ρ为地球的偏心率，φ
ρ22sin 1-=e
R N 。

（2）从地理坐标()h ,,φλ到传感器局部坐标[]T z y x 的变换关系为
()[]),,(),,(,s s s s s h P h P D z y x φλφλφλ-⋅=⎥⎥
⎥⎦
⎤
⎢⎢⎢⎣⎡
其中，),,(s s s h φλ为传感器的地理坐标，而
()⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡---=s s
s s
s s s s s
s s s s s D φλφλφφλφλφλλφλsin sin cos cos cos cos sin sin cos sin 0cos sin , （3） 从传感器局部坐标[]T z y x 到地球固定坐标T Z Y X ][的变换关系为
()()⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡⋅+=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⋅+=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-z y x D h P z y x D h P Z Y X s s T s s s s s s s s φλφλφλφλ,),,(,),,(1 （4）地球固定坐标[]T e e e Z Y X 到地理坐标()h ,,φλ的变换
从地球固定坐标[]T e e e Z Y X 到地理坐标()h ,,φλ的变换较为复杂，求纬度时用到迭代。

下面给出的是一种求取地理坐标的算法。

e
Y Z
Z
,X
图1 参考的椭球及地理参数
图2 椭圆和地理纬度
假设:
地球的长轴半经: km R e 137.6378=; 地球短轴半径为km r e 7523142.6356=;
e e e R r R
f /)(-=;
λ：地理经度
φ：地理纬度 h ：高度
P 点的直角坐标：)(e e e Z Y X
P 点的经度可以直接求出 e
e
X Y a tan
=λ 考虑地球椭圆切面（如图2），有
122
22=+e
e r Z R X
在OXZ 坐标系下，P 点的坐标为 220e e Y X X +=，e Z Z =0 椭圆方程的极坐标形式（或1P 的坐标）为
⎩⎨⎧==θθ
sin cos 11e
e r Z R X ， πθπ<≤-
1P 点处椭圆切线的法向量为（e e r R /sin ,/cos θθ）
θ
θ
θθφcos sin /cos /sin tan e e e e r R R r ==
与1P 点处椭圆切线垂直并通过1P 和P 点的垂线为
θ
θ
θθcos sin cos sin 00e e e e r R R X r Z =--
)2/tan(θ=t ,则上述方程可写为
0)()()(34=--+++=A t C B t C B At t f 其中
0Z r A e =，02X R B e =，)(222e e r R C -=
上述方程为一元4次方程，可以通过复杂的推导得到其解析解。

方程的根也可以通过Raphson Newton -（NR ）方法近似求取。

这里，我们采用NR 方法求取
,2,1,0),(/)(1='-=+k t f t f t t k k k k
其中，)()(34)(23C B t C B At t f -+++='。

现在的问题是如何求取0t 。

因为
011
tan X Z R r X Z e e ≈=θ （由于11,Z X 未知，而00,Z X 已知，1PP 与PO 线的斜率近似相同）
取
00tan X Z
R r e e =θ，并令)2/tan(00θ=t ，得 0)1(2000200=--+Z t X f t Z 解方程，得
2
000000)1(1)sgn()1(⎥⎦⎤⎢⎣
⎡-++--=Z X f Z Z X f t
其中，)sgn(0Z 是符号函数
⎩⎨
⎧<-≥=0
1
01
)sgn(000Z Z Z
由0t 通过迭代可求出k t ，并取k t t =，再计算出纬度φ和高度h 。

⎥⎦
⎤
⎢⎣⎡--=t t f a 21)1(cot 2φ 因为
φsin 10h Z Z += 于是
2
22021)1(112)sgn(⎥⎥⎦
⎤
⎢⎢⎣⎡--+⎪⎪⎭⎫ ⎝
⎛+-=t t f t t
r Z h e φ 假定δ为迭代精度，m ax k 为最大迭代次数。

（1）
计算00,z x 和系数C B A ,,
()
2
20002
2022e e e e e e e r R C X R B Z r A Z Z Y X X -====+=
（2）
如果00=Z
e
e
e R X h X Y a -===00
tan φλ
转入（8） （3）
初始化k 和k t
0=k ⎩⎨
⎧<-≥=0
1
01
)sgn(000Z Z Z
()2
0000011)sgn()1(⎥⎦⎤
⎢⎣
⎡-++--=Z X f Z Z X f t k
（4）
1+=k k
A t C
B t
C B At t f k k k k --+++=----13
1411)()()(
)()(34(2131)1C B t C B At t f k k k -+++='---
()
()
111---'-
=k k k k t f t f t t （5）
如果δ≤--1k k t t ，转入
（7） （6） 如果max k k <，转入（4） （7）
2
22
02
21)1(112)sgn(01
01
)sgn(21)1(cot tan
⎥
⎦⎤⎢⎣
⎡--+⎪⎪
⎭⎫ ⎝⎛+-=⎩⎨
⎧<-≥=⎥⎥
⎦⎤
⎢⎢⎣⎡--==k k k k
e k k e
e t t
f t t r Z h t t f a X Y a φφφφφλ。