多传感器数据融合及其应用(杨万海)第2章.ppt

i=x, y, z （2-1-98） σ2ai为x, y, z轴方向的加速度扰动， 分别是距离、方位角和高低角方向的噪声方差。 显然，H（k+1）是三点启动时的化简： （2-1-99） 2.1.6 卡尔曼滤波器在雷达跟踪中的应用 1. 系统矩阵 假定系统矩阵是四维矩阵，即距离、速度、方位角及其变化率, 它们分别由R， ，θ和 表示，距离方向上的加速度和角度方向的加速度分别由ur(k)和uθ(k)表示。状态方程为 (2-1-100) 则系统方程为 (2-1-101) 用标准符号x1，x2 ，x3, x4分别表示 R，R, θ, θ。式(2-1-101)中， A为系统矩阵，W(k)为噪声项。 . . 2. 观测矩阵 假定观测值只有距离和方位两个，即R和θ，分别用z1和z2来表示。它们是由状态值和测量噪声组成的，且测量噪声是相互独立的零均值的白噪声。  测量方程： (2-1-102) 则有 其中， x1(k)=r(k)，x3(k)=θ(k)。  以上两个问题实际上是建立模型问题。 3. 观测噪声协方差矩阵 在计算滤波器增益时，需知观测噪声的协方差矩阵。由于只有两个参数，因此 (2-1-104) 这里利用了方位和距离观测噪声相互独立的条件，故左下角和右上角项为零。 4. 系统噪声协方差矩阵 假定目标作匀速运动，但由于大气湍流等因素的影响， 目标产生随机加速度，在距离和方位上都存在随机扰动, 于是有 且 因为 得输入扰动的协方差矩阵 (2-1-105) 5. 滤波器的初值 在运算之前，必须对滤波器进行初始化。首先利用一种比较简单的方法确定 ， 可利用时刻1和时刻2两点的距离和方位测量值，即z1(1)，z1(2)，z2(1)， z2(2)， 建立 ，而忽略随机加速度。 (2-1-106) 6. 均方误差矩阵 由滤波器初值，有误差矢量 从而， （2-1-107） 初始误差的协方差矩阵 (2-1-108) 由于u，v相互独立，且各噪声采样之间也独立， 则 (2-1-109) 式中, 这样，所需要的参数均已具备，可以进行迭代运算了。 2.1.7 扩展卡尔曼滤波器在目标跟踪和卫星轨道确定方面的应用 1. 目标跟踪  假设被跟踪的目标是在二维空间内运动，要根据所测量的距离和方位数据实现对目标的跟踪。所采用的坐标系为直角坐标系［x(t)，y(t)］，其速度以 表示，目标状态表示为四维矢量： （2-1-110） 我们把目标的加速度看作高斯白噪声，目标的动态方程便可以表示为 （2-1-111） 其中， W(t)是连续时间二维高斯白噪声矢量过程，并有 其中Q是2×2阶协方差矩阵。 观测是由观测者(也可能是雷达系统)进行的，它位于坐标原点。观测数据包括距离r(ti)和方位θ(ti)。 (2-1-112) 距离以米表示，方位以弧度表示（1rad=57.295 78°）, 如图2-10所示。 图2-10 一个简单的目标跟踪 我们假设，观测夹杂着高斯白噪声，于是就可以将观测矢量Z(tk)写成 (2-1-113) 其中Vk是二维离散时间高斯白噪声矢量过程，有 每5 s采样一次，如表2-1所示。下面的参数用来产生这些数据序列： 表2-1 目标跟踪问题的数据 为了验证程序的正确性，利用以下给出的初始估计和初始协方差矩阵: 并利用R，Q的值，得出滤波器的列表输出。 利用该程序确定不同的R值对估值的影响。并讨论什么样的R值是最好的。 2. 卫星轨道确定 这里的任务是确定围绕地球运行的卫星的轨道。 我们假设卫星被限定在一个圆形轨道上运行，并且把地球看作为一个质点。卫星的二维位置和速度由下面的参量描述： v(t)——卫星速度，以km/s表示； γ(t)——由本地水平线测量的飞行路径角，以rad表示； r(t)——卫星对地球中心的距离，以km表示； φ(t)——卫星与赤道的夹角，以rad表示。 图2-11 卫星状态参量 在没有扰动的情况下，卫星的轨迹可由以下差分方程确定： （2-1-114） 其中，μ=398 602.8225 km3/s2是地球的引力常数。卫星的状态由下式表示： 其动态特性 (2-1-115) 式中，W(t)表示未知扰动， 如空气阻力、地球引力场变化等。 我们把W(t)