第二章 状态估计基础

第二章 状态估计基础状态估计的目的是对目标过去的状态进行平滑、对目标现在的运动状态进行 滤波和对目标未来的运动状态进行预测。这些运动状态包括目标位置、速度、加 速度等。本章讨论在多传感器跟踪系统中广泛应用的状态估计技术，这些技术包 括Kalman滤波技术，a -卩滤波与a -卩-丫滤波技术、最小二乘滤波技术和非 线性系统的状态估计技术。线性系统估计卡尔曼滤波技术线性系统描述1. 离散时间线性动态系统的状态方程线性系统采用状态方程、观测方程及其初始条件来描述。线性离散时间系统的 一般状态方程可描述为X (k +1)=(k) X (k) + G (k )V (k)其中，X (k) G Rn是k时刻目标的状态向量，V (k) G Rn是过程噪声，它是具有均值为零、方差矩阵为Q(k)的高斯噪声向量，即V (k )= 0V (k )Vt (l )=Q(k )8kl(8 ：狄拉克函数，或单位冲激函数)，(k ) G Rnxn是状态转移矩阵，G (k ) G Rnxn是过程。2. 传感器的测量(观测)方程 传感器的通用观测方程为Z (k) = H (k) X (k) + W (k)这里，Z(k) G Rm是传感器在k时刻的观测向量，观测噪声W(k) G Rm是具有零均值和正定协方差矩阵R(k)的高斯分布测量噪声向量，即eW (k )=0EW (k )Wt (k )=R(k )8kl3初始状态的描述初始状态X(0)是高斯的，具有均值X(010)和协方差P(01 0)，即eC (0) - X (010)1 (0) - X (010I P(010)以上描述比较抽象，下面结合具体的例子加以说明。例 1：目标沿 x 轴作匀速直线运动，过程噪声为速度噪声，试写出目标的状态方 程。解：目标的状态为X (k)=x(k) X(k)用 T 表示时间间隔，u 表速度噪声，则有 xX(k)=x(k)X(k)y(k)y(k)=L(k) X(k) y (k) yk )bx(k +1) = x(k) + TX(k) + Tu (k) xX(k +1) = X(k) + u (k)x写成矩阵形式为1 TVx(k)+T0 1入yk)1x(k +1)玻k +1)u (k)x1厂, G =丁0 11(k)=V(k) = u (k)x有X (k +1)=(k) X (k) + G (k )V (k)其中Q(k)= EV(k)VT(k)= Eu2(k)=q xu (k):均值为0,方差为q的高斯噪声。x例 2:目标为二维空间中作匀速直线运动的目标,过程噪声为加速度噪声,试写 出目标的状态方程。解:由于目标为二维空间作匀速直线运动的目标,目标的状态中有目标的位置和 目标的速度,那么目标的状态可写为用T表示时间间隔，u ,u分别表示x, y方向的加速度噪声，则有xyx(k +1) = x(k) + TXk) + u (k)2xVy (k +1) = y (k) + Ty(k) +Xk +1) = Xk) + Tu (k)xT2u (k)2 yXk +1) = Xk) + Tu (k)y写成矩阵的形式有x(k + 1)Xk +1)y (k +1)_ Xk +1)_1000T100001000T1x(k) Xk) y(k) yX(k)T 22T0T 2TTu (k)xu (k)y令 X (k)=x(k) xX(k) y(k) yX(k),(k)=1000T100001000T1G (k)=0T 2TT,V (k)=u (k)xu (k)yX (k +1)=(k) X (k) + G (k )V (k)Q(k) = E=E u (k)xu (k)y(k) uxyIE (u 2( k)=IxI E(u (k)u (k)xyE(u (k)u (k)xyE(u2(k)y假定u (k), u (k)为均值为0,方差分别为q和q的相互独立的高斯白噪声，则x yx yQ(k)=E(u2 (k)xE(u (k )u (k )xyE(u (k )u (k)xyE(u2 (k)y例3： 目标为沿x轴做匀加速运动的目标，过程噪声为加速度噪声，试写出目标 的状态方程解：目标的状态可写为：Ix(k)X1(k ) = IIxX(k )IIXx(k)用T表示时间间隔，u ,u ,u分别表示x, y,z方向的加速度噪声，则有 xyzx(k +1) = x(k) + TX(k) + X(k) + u (k)2 2 x X(k +1) = X(k) + TW) + Tu (k) xX(k +1)=粼)+ u (k)x写成矩阵的形式有x(k +1)X(k +1)0.5T 2Txx(k) 璞k)+0.5T 2T1处）11T0100x u (k)x1 T0.5T 20.5T 2(k)=0 1T, G (k) =T, V (k) = u (k)10 01111xX (k +1)=(k) X (k) + G (k)V (k)1 1 1 1 1其中，Q1(k)=Exq为x方向加速度噪声的方差。x例 4：在例 3 的基础上，假定目标为三维空间中作匀加速运动的目标，过程噪声 为加速度噪声，试写出目标的状态方程。解：由于目标为三维空间作匀速直线运动的目标，目标的状态中有目标的位置和 目标的速度，那么目标的状态可写为X (k )X(k)=1X (k )2X (k )3其中x(k)-y (k )z (k)X (k) =1X(k), X (k) =2y(k), X (k) =3Zk)1处）2处）3鍬）目标的状态方程可写为X (k +1)=(k) X (k) + G (k )V (k)其中(k)i(k) =000(k)i000(k)iG(k)iG (k) =000G (k)1000G (k)1V (k)=u (k)xu (k)yu (k)z而过程噪声协方差矩阵为qxQ(k)二 000qy000qzq ,q ,q分别为x方向，y方向和z方向加速度噪声方差。 xyz例 5：假定对二维空间作匀速直线运动的目标进行观测时，观测值为目标的位置 加上观测噪声，试写出目标的观测方程。解：由前面可知，二维空间中作匀速直线运动的目标，其状态向量为X (k)=x(k)X(k)y(k)黄k)由题意得目标的观测方程为fz (k) = x(k) + w (k)xxz (k) = y(k) + w (k)yy将上式写成矩阵的形式，有z (k)-1000_x k)w (k)x+xz (k)y0010y (k)w (k)y其中w (k), w (k)分别为x和y方向的观测噪声。x(k)(k)xy令 Z (k)=z (k)xz (k)y000,W (k)=(k)(k)，则有Z (k) = H (k) X (k) + W (k)R(k)二 eW(k)Wt (k)L Ew (k )xw (k )y(k)xywy(k )w (k ) yy假定w (k), w (k)为均值为零，方差分别为r ,r的高斯白噪声，则R(k)二xy例 6：假定对三维空间作匀速直线运动的目标进行观测时，观测值为目标的位置 加上观测噪声，试写出目标的观测方程。解：由前面可知，三维空间中作匀速直线运动的目标，其状态向量为X (k)=x(k) 蝕) y(k) y(k) z(k) Z(k)由题意得目标的观测方程为z (k) = x(k) + w (k)xx z (k) = y(k) + w (k)yyz (k) = z(k) + w (k)zz其中 wx(k), w (k),w (k)分别为x,y和z方向的观测噪声。将上式写成矩阵的形 y式，有x(k) X(k) y(k) y(k) z(k) 敢k)w (k)xw (k)yw (k)zZ (k) = H (k) X (k) + W (k)假定w (k), w (k), w (k)为均值为零,xyz方差分别为 r ,r ,r 的高斯白噪声，则xyz00rzr 0xR(k) =0 ry0 0作业：假定对三维空间作匀加速运动的目标进行观测时，观测值为目标的位置加 上观测噪声，试写出目标的观测方程。例7：设目标沿x轴匀速直线运动，目标的状态可表示为X = tx xT，在t时刻0的x观测值为z(0)，在t寸刻的x观测值为z(1)，采样间隔为T = t -1，求目标1 1 0的初始状态和初始协方差。 解：初始状态为入z(l)X (111) = z (1) - z (0) T 初始协方差r r / TP(111)二r / T 2r / T 2其中，r为观测噪声的方差，即：W (k) T N (0, r)，滤波器从k = 2时开始工作。Kalman滤波算法状态估计的一步预测方程为X (k +11 k)二(k) X (k I k)一步预测的协方差为P(k + 1I k)二(k)P(k I k)Qt (k) + G(k)Q(k)Gt (k)预测的观测向量为为(k + 1I k)二 H (k +1) X (k + 1I k)观测向量的预测误差协方差为S(k +1)二 H(k +1)P(k + 1I k)Ht(k +1) + R(k +1)新息或量测残差为v(k +1)二 Z(k +1)-为(k + 1I k)二 Z(k +1) - H (k +1)X (k + 1I k)滤波器增益为K(k +1)二 P(k + 1I k)H t (k +1)S -1 (k +1)Kalman滤波算法的状态更新方程为X (k + 1I k +1)二 X (k + 1I k) + K(k + 1)v(k +1)滤波误差协方差的更新方程为P(k + 1I k +1)二 P(k + 1I k) - K(k +1)S(k +1)Kt (k +1)=b - K(k +1)H(k + 1)p(k + 1I k)转换坐标卡尔曼滤波器（非线性估计技术）。卡尔曼滤波器两个要求：1） 目标的状态方程是线性的；2）观测方程是线性的。在实际的情况下，要同时满足这两个要求是困难的。通常情况下：状态方程 在直角坐标系下是线性的，而观测方程是在极坐标系下获得的关于目标的测量。 如雷达的测量是距离、方位角和高低角。从直角坐标系来看，观测方程是非线性 的。假定雷达的测量为距离r、方位角0和高低角，测量与目标位置的关系为r = ix 2 + y 2 + z 2Lxyzxyz xyzr厂一vrvLvvl0r08vxAT假设距离、方位角和高低角的测量噪声为相互独立的高斯白噪声，即E（八p）二 E（v p 叮-E（八&）二 E（vs 叮二 E（v p 叮二 E （vsv 卩）二 0并且rpp2,s 2x2xy 2xz 2r00 _R= 22 2= A x020xyzyxyyzP 22 200 2zxzyz-sx AtQ 2s 2, r对两坐标雷达，极坐标系下的观测为(r,9 ),转换成直角坐标为fx = r cos Py = r sin P方差的转换：为求方差的转换，先求偏导dxdycos Pr sin Pdr=Adrsin Pr cos P_祁_dP_Tdx idxdydrdrdrdpdpdpTIdr于是，得xy2x 22xy20At 2xy y 2 = 2 cos2 P + r 2Q 2 sin2 PxrP 2 = 2 sin 2 P + r 2Q 2 cos2 P yrP / 2 = 2 = sin P cos P o 2 一 r 2Q 2 丿xyyxPP在得到直角坐标系下的测量及测量的协方差矩阵后，可以用前面讲的方法进行目 标跟踪。这种变换存在的问题是：在直角坐标系下，测量的噪声不再是严格意义上 的高斯噪声，并且噪声的分布中心并不是以直角坐标 (x,y,z) 为中心，而且在直角坐标系下噪声是相互关联的。Ex|r, Px丰y由于噪声的分布中心并不是以直角坐标(x,y, z )为中心，因此，噪声是有偏 的，为此，有些学者提出了无偏坐标转换的方法：对转换后的直角坐标和方差进 行校正。通常情况下，传感器的测量为极坐标测量，极角坐标测量Z可通过无偏坐标 转换变为直角坐标测量。设Z = lr 0，则无偏转换的测量z c为XcX-i r cos 00_yc _X-i r sin 0L 0zc其中，丁 e诩2。转换测量的协方差为RcRc11Rc21Rc12Rc22其中Rc =(九-2 一 2)r2 cos2 0 + (r2 +c 2)(1 + 九4 cos20)11 02r0Rc =(九-2 一 2)r2 sin2 0 + (r2 +c2)(1 九4 cos20)2202r0Rc = Rc =(九一2 一 2)r2 cos 0 sin 0 + (r2 +c 2)X4 sin2012 2102r 0在动态方程为线性、观测方程为非线性的情况下，可使用转换坐标卡尔曼滤 波器，转换可采用两种方法：一种是不带校正量的转换，这是一种相对简单的方 法；另一种为带校正量的方法，这是一种相对复杂的方法。扩展卡尔曼滤波器(非线性估计技术) 采用扩展的卡尔曼滤波器进行目标跟踪 2.3.1 系统的状态方程和测量方程 状态方程：X (k +1)=(k) X (k) + G (k )W (k)其中EW (k) = 0EW (k )Wt (j) = Q(k )5kj(k)为状态转移矩阵。测量方程：Z(k +1)二 hx (k +1)+ V(k +1)其中EV (k) = 0EV (k )Vt (j) = R(k )5kj、观测方程线性化将等号后的第一项以文(k +11 k)为中心按泰勒级数展开并略去二级以上的高阶分量，有hX (k +1)二 hX (k + 1I k) + 色dX丄(k +1) - X (k+1I k)x=X (k+1Ik)代入上式，得Z (k +1)二 hX (k + 1I k) +等丄(k+1) - X (k+1I k 丄 V (k+1)x=X (k+1Ik)Z (k +1) - hX (k + 1I k)二寮丄(k+1) - X (k+1I k J+ V (k+1)X=X (k Ik -1)5X=X (k+1Ik)Z(k +1)二 Z(k +1) - hX(k + 1I k)，X(k + 1I k)二x (k +1) - x (k + 1I k)，于是得到线性化的观测方程为Z(k +1)二 H(k +1)X(k + 1I k) + V(k +1)假定目标的状态为X二x ,A ,x T，而h(X) = lf (X),A , f (X)h 则1 n 1 mH (k +1)=f (X)dx芳(X)mdx1f (X)dxaf ( x )maxnx=X (k+1Ik)为h(X)的雅可比矩阵(Jacobian矩阵)。例：雷达的观测是在极坐标系下进行的，对于一个直角坐标为(x,y,z)的目标, 雷达所测的三个极坐标分别为Ir = f (x, y, z) =、x2 + y2 + z2yp = f (x, y, z) = arctan 2x”厶（X，y，Z）=arctan观测方程为Z (k +1)=甘 x 2 + y 2 + z 2arctan xarctan zIx.x2 + y2h(X)=+ V (k +1)=灿 X (k +1) + V (k +1)vx 2 + y 2 + z 2arctan xzarctanx2 + y2令目标的状态为X = x仪鈕y敷數z&脚，则线性化的观测矩阵为dx6& 。鈕f f f6x 6&。鈕6區f6典X=X (k +llk)x00200zrrr-y00x000x2 + y 2x2 + y 2-xz00yz00x2 + y2r 2 x 2 + y 2r 2 廿 2 + y 2r2000000X=X (k+1lk)f f f6衣6疊6zf f f6衣6疊6zf f f6衣6疊6z其中x 2 y 2 + z 2例：红外传感器，观测值为方位角p和高低角,求目标的状态为X = x y典z 曲时线性化的观测矩阵arctan 解：因 h( X)=arctanQh-yx 2 + y 2H (k +1)=QXX=X (+1k Ik)-xzr 2 Jx2 + y22.3.3、扩展卡尔曼滤波方程一步预测：0x00x2 + y 20yz0vx2 + y2r2x2 + y2r200X = X (k+1Ik)X (k +11 k)二(k) X (k I k)一步预测协方差矩阵P(k + 1I k)二(k)P(k)Qt (k) + G(k)Q(k)Gt (k)预测的观测向量为N (k + 1I k)二 h X (k + 1I k)观测矩阵H (k +1)二色OXX=X (k+1lk)新息或量测残差为v(k +1)二 Z(k +1) - Z(k + 1I k)二 Z(k +1) - hX (k + 1I k)残差协方差矩阵S(k +1)二 H(k +1)P(k + 1I k)Ht (k +1) + R(k +1)滤波器增益K (k +1)二 P(k + 1I k)H t (k +1)S -1 (k +1)滤波输出X(k + 1I k +1)二 X(k + 1I k) + K(k + 1)Z(k +1) - hX(k + 1I k)误差的协方差阵P(k + 1I k +1) = I - K(k +1)H(k + 1)P(k + 1I k)例题2-2：利用扩展的卡尔曼滤波算法对 y 轴上匀速运动的目标进行跟踪。设传感器平台在 x-y 平面内运动，运动方程为 = -50000 + dx = 4t + dy式中，t为时间,Sdx和dy为相互独立的、零均值白色高斯噪声，其方差分别为 ssr =1m2,r =1m2 ，且与过程噪声和量测噪声相互独立。过程噪声为加速度噪声， xy其方差为b 2 = 16 Cm/s2），传感器测量目标的方位角，采样时间间隔T = 2 s，测v量噪声为零均值的咼斯噪声，其方差为r = （3。）2 =(3 x 3.14159180rad丿解：目标在y轴上运动，系统的状态方程为X (k +1)=(k) X (k) + G (k )v(k)其中，X(k) = y(k) y(k)1(k)=1T01G (k)=T2TT传感器的测量方程为z (k) = h X (k) + w(k)其中，h() = arctan=arctan建立目标位置估计值y （k）和方位角测量值z （k）之间的关系（不考虑测量误差）为z(k ) = arctany (k) - y、一x丿S所以y(k) = y (k) - x (k) tan(z(k),由 z(0), z(1),AT y(0), y(1),Ass目标的初始状态对上式求导可得dy - dy - dx tan( z(k)-ssxcos2(z(k)dz于是=c2 + c2 tan2(z(k) + yxssx2s c 2cos4 (z(k) z初始状态的方差线性化的观测矩阵=r + r tan 2(z(k) + yxx 2cos4( z (k)P(1|1)=c2yc 2 /T-y(i)c 2 /Ty2c 2 /T 2yH (k +1)= hx (k + 1)=罟。灿 X X=X (k+1lk)-xsx2 + (y - y )20_X=X (k+1lk)ss无迹卡尔曼滤波器(Un see nted Kalman Filter, UKF)目前，扩展卡尔曼滤波虽然被广泛用于解决非线性系统状态估计问题，但其 滤波效果在很多复杂的系统中并不能令人满意。模型的线性化误差往往会严重影 响最终的滤波精度，甚至导致滤波器发散。另外，在许多实际的应用中，模型的 线性化过程比较繁杂，而且也不易得到。无迹卡尔曼滤波是由于不需要对非线性系统进行线性化，并可以很容易地应 用于非线性系统的状态估计，因此，UKF方法在许多方面得到应用。2.4.1 无迹变换无迹卡尔曼滤波是在无迹变换的基础上发展起来的。无迹变换(Un sce nted Transformation, UT)的基本思想是由Julier等首先提出的，是用于计算经过 非线性变换的随机变量统计的一种新方法。它不需要对非线性状态和测量方程模 型进行线性化。假定X为一个n维的随机变量，g : Rnx 一 Rny为一非线性函数，并且XY = g(X)。X的均值和协方差分别为X和P。无迹变换的一般步骤如下：X(1)确定(2n +1)个5采样点X+九)P )、XX i )+ X) PX ii = 0i = 1,A , nXi = 1,A , nXi=0i = 1,A , nXi = 1,A , nX=X; /=X+V=X仝莎i+nxXWo = /(n + 九)；+ 九)P 为 v： (n + X)PXX i k XXXPXW (Y - Y)(Y - Y)TYi iii=02.4.2 无迹 Kalman 滤波算法根据无迹变换的特点，Kalman滤波算法可归纳如下：假定状态方程：X (k +1) = fX (k) + G(k)V(k)观测方程：Z(k +1) = hX (k) + W(k)k时刻的状态估计值和状态协方差：文(k I k)和P(k I k)k时刻的观测向量Z(k +1)1) 根据采样规则，确定 2n +1 个采样点以及相应的加权值XX = X (k I k); /、0八()X = X (k I k) + (n + 九)P(k I k 、iZJX= X(k I k)-(n + 九)P(k I ki+xx+ 九)P(k I kXIi =1,A ,nXi =1,A ,nX/(n + 九)X,-2(n + 九)X=J2(n + 九)X其中，为尺度参数，可以为nX朴丰0的任意值。莎E)为(n +九)P(k-II k-1)均方根的第i列。n为状态向量X的维数。XX2) 一步预测X (k + 1I k) = fX ； i = 0,A ,2niiXX (k + II k) = 2nXWX (k + II k)iii=0P (k + 1I k) = G(k)Q(k)GT (k) +込 W & (k + 1I k) - X(k + 1I k)xx (k + 1I k) - X(k + 1I k)t XXi iii=0Z (k+1Ik)=hX (k+1Ik)； i=0,A,2niiXZ(k + II k)二込WZ (k + II k)iii=0P (k + 1I k) = R(k +1) + 迓W fa (k + II k) - Z(k + II k)xz (k + II k) - Z(k + II k)bZZi iii=0P (k + 1I k)=込W lx (k + 1I k) - X(k + 1I k)xz (k + 1I k) - Z(k + 1I k)tXZi iii=0(3) 更新状态K(k +1) = P (k + 1I k)P-i(k + 1I k)XZZZX (k + 1I k +1) = X (k + 1I k) + K(k +1)(Z(k +1) Z(k + 1I k)P(k + 1I k +1) = P (k + 1I k) K(k +1)P (k + 1I k)Kt (k +1)XXZZ在动态方程和观测方程同时为非线性、或者一个为线性另一个为非线性的 情况下，可以使用无迹卡尔滤波器，无迹卡尔曼滤波器的缺点是计算量较大。与 无迹卡尔滤波器类似的滤波器还有积分卡尔滤波器(QKF)和粒子滤波器，这些 滤波器均可用于非线性系统，其缺点是计算量更大。a - P和a - p -y滤波器a-p和a-p-y滤波器分别用于对匀速和匀加速目标进行跟踪。2.5.1、a - p 滤波器a - p滤波器用于对匀速运动的目标进行跟踪。目标的状态方程为X (k +1) = 0X (k) + GV (k)式中，X(k)=T，Ely(k)V，(j)=Q25Tv k目标的观测方程z (k) = HX (k) + W (k)式中，H =1 q， eW (k )W )= 2 5W kj滤波方程X (k +11 k +1)二 X (k +11 k) + K lz (k +1) - z (k +11 k) 二 X(k +11 k) + B ?b(k +1) - Z(k +11 k) 问题：如何求a和0，利用稳态滤波器的特点，即lim P(k I k) = lim P(k + II k +1)k Tgk fglim K (k)三kTga0/T九2 + 8九一（九+ 4） J九2 + 8九8九2 + 4九一九丫九2 + 8九0 二其中，沁二竺，九为目标的机动系数。G 2Wa_0/T状态估计X (k + 1I k +1)二 X (k + 1I k) +状态估计的协方差为0G 2 TW0G 2 TW0 (a 0)/2T 2(1 -a)步预测协方差为lim P(k + 1I k) = M =kTgO 2T (1-a) WO 2T (1-a) Wa0T 2O 2 + O 2T 2 (1- a) W 2 W以上只讨论的是沿x轴作直线运动情况下的a-0滤波，对于平面或空间目标，需要分别对X、y及Z上的状态向量分别d 卩滤波。当过程 噪声方差不能事先确定时，目标的机动系数无法确定，d和B两参数 无法确定。工程上经常采用两种的方法来确定d和B :1、常系数法：d和B取固定的值般取d二0.30.5B = 2-d -2 J1 -d （临界阻尼法）（最佳选择法）2、变系数法：2(2k 1)k (k +1)6k (k +1)这里k从1开始计数。对P来说k二2时才有值，但滤波器从k二3开始工作，前两个点用于确定目标的初始位置和速度，完成航迹起始。2.5.2 d p y 滤波器d 卩y滤波器用于对匀加速运动的目标进行跟踪。目标的状态方程为X (k +1)=X (k) + GV (k)式中，X1 T T 2j2T 2,/2X (k)=,0 二01T,G =T0 0 11,eV(kv(j)=Q 2V kj目标的观测方程z (k) = HX (k) + W (k)式中，H = 1 0 o, EW(k)W(j)=Q26W kj滤波方程X (k +11 k +1) = X (k +11 k) + K z(k +1) - z(k +11 k)a=X (k +11 k) +卩/T lz(k +1) -z(k +11 k)Y/T 2问题：如何求a、p和丫，利用稳态滤波器的特点，即lim P(k I k) = lim P(k +11 k +1)k Tgk fglim K (k)三kTgap/TY /T 2得三个非线性方程构成的方程组Y 2=九24(1 -a) 0例 ： 设 目 标 作 匀 速 直 线 运 动 ， 目 标 的 状 态 可 表 示 为lx(t)X(t)y(t)y(t)，目标的起始状态为x(t)X(t)y(t)y(t),目0000标的起始时刻为t ,在t时刻的观测值为(x, y ),在t时刻的观测值为0 1 1 1 2(x ,y ),观测噪声为相互独立的高斯白噪声，a =a = 10m ,分别求t22x y0和t时刻目标状态的最小二乘估计。2a、求t时刻目标状态的估计值0解：00 _x(t )0vxl1t - t玻t )v100+yi00y (t )0vx 21t t20y(t)0vy2Y 二 CX + V得到估计： X(t ) = (CtC)-iCtY0估计误差的方差： P(t )二(CtC)-1 CtRC(CtC)-1 0其中R = Var(V) = E (VVt )=Q2 Q2yxQ 2)= diag(100 100 100 100)yb、求t时刻目标状态的估计值2x1y1x2y2t - t00 _x(t )v122x101t - tX(t )v122+y1000y (t )v2x 2010(t )2vy21010Y = CX + V得到估计： X(t ) = (CtC)-1 CtY2估计误差的方差： P(t ) = (CTC)-1CT RC(CTC)-1 2其中R=Var(V) =E(VVT)=2 Q2 Q2 Q2xyxy)=diag(100 100 100 100)2.6.2、加权最小二乘估计当测量数据的测量精度不等时，应采用加权处理，对精度较高的 测量结果赋以较大的权。目标：求X以使工 VtV =Lw (Y - CX) T (Y - C X) = mini ii i ii ii =1i =1得到估计： X = (CtWC ) -1 CtWY估计误差的方差： P = (C TWC)-1CTWRWC(CTWC)-1其中 R = Var (V) = E (VVt ) 02.6.3、马尔可夫估计马尔可夫估计是一切加权最小二乘估计中能使得估计方差阵达到最小的一种。当取W = R 一 1时，加权最小二乘估计为马尔可夫估计估计值为： X = (CtR - iC) -1 CtR - iY其估计误差的方差为P 二(CtR一iC)一 12.6.4、非线性最小二乘估计模型：Y = f (X) + Vi = 1,2, A , mi iiY = F (X) + V目标：求X以使艺 VtV 二区(Y - f (X)t (Y - f (X) = mini ii ii ii=1i=1另一种表示X = arg min(Y f (X) t (Y f (X)X I丿i=1加权最小二乘估计目标:区c VtV =Ec (Y - f (X)T(Y - f (X) = mini i ii iiiii =1i =1令Y = =,f (X)= 竺，非线性加权最小二乘简化为非线性最小二乘 .v；c.c.区(Y1 - f i(X)t(Yi - f i(X) = min。iiiii =1非 线 性 最 小 二 乘 估 计 可 以 用 Gauss-Newton 算 法 或 Levenberg-Marquart 算法(这些算法在 Matlab 中都已编好，直接应 用就行，如Isq non li n，当然也可自行编写)求解。这里给出一种自 我实现的方法。Y-f( X)1 1E (X) = M|_Y - f (X)mmJ (X)=于ax tdE (X )-pdF(X)dxdXTdXTafaxaf af afa& ayayMMM af af af7/1/17/1/17/1/1IIII I Vlita&ayaya&Mafma&则Levenberg-Marquart算法求解非线性最小二乘的迭代公式为X = X -Jt(X )J(X ) + u I】iJt(X )E (X )kk -1k-1k -1k -1k -1 p k -1令A 二 J T(X )J (X)一种伪语言描述的LM算法如下：T = io-3 ;e1:=10-15 ;e2:=10-15; e3:=10-15 ;kmax=100;k:=0;v:=2;p=X;0A:=JT*J;Ep:=Y-F(p);g:=JT*Ep;Stop: = (|g ei);|Li =t * max(A )；i=1,A , m iiwhile(not stop)and(kkmax)k:=k+1;repeatSolve (A+(1 I)* =gif (p p| 0p:=pnew;A:=JT*J;Ep:=Y-F(p);g=:JT*Ep;Stop: = (训 el)or |E|2 0)or(stop)endwhileX:=p;非线性最小二乘估计也是一种最大似然估计，是一种最优估计， 估计误差的方差能达到CRLB,因此估计误差的方差可取为CRLB。估 计误差的方差为P u Jt(X)J(X J1例：匀速运动目标，目标的状态为X(t) = lx(t) X(t) y(t) y(t)，t时刻 的测量为r e r，t时刻的测量为r e r，传感器测距误差标准差 1 1 2 2 2为b =10m,测角误差标准差为b二,r9设t 时刻为 X(t ) = L(t ) Xt ) y(t ) 2210002x(t )1Xt)1y(t )1yX(t )1t t11000010200t t121求 t 时刻目标的状态。2Xt )t ,贝时刻目标的状态X为2 1 1x(t )2X )2y (t )2X )2t 时刻的理论观测值为1C -1)Xt ) )1 + Ky(t ) + C -1)Xt ) )11 2 2 2 1 2 2f (v( ) t y(t) I t y(t ) + C t)912(x(t )丿(G(t ) + 0 -1)Xt ) /VX (t2)V y V)ft 时刻的理论观测值为2f (X(t ) = ：x2(t ) + y2(t )r 2222f02( X (t 2)=啊器 j加权非线性最小二乘估计的目标函数为B X (t 2)=(r- f(X(t)2+ 丄(0- f(X(t)2+ 丄(r- f (X (t )+ 丄 -b 21r12b 21912b 22 r 22b 22r9r992(X(t2)2t 时刻目标状态 X 的估计为22X = arg min(B(X (t )22Matlab 求 Jacobian 矩阵的方法syms x xv y yv dt std_r std_b r1 b1 r2 b2y1 = (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维雷达，测量目标的距离和方位角，传感器位置为1000 m,0 mT,其测距误差标准差为100 m,测角误差标准差为弧度。在t二0 s时，观测值为, 0在t = 10 s时，观测值为,，在t = 20 s时，观测值为,，分为用12 线性最小二乘和非线性最小二乘法估计目标的初始状态和状态估计 的协方差。2.6.5、最大似然和非线性最小二乘的转化例：匀速运动目标，目标的状态为X = & y y&T，t时刻的测量为 r 0r，t时刻的测量为r 0丨，传感器测距误差标准差为b =10m, 测角误差标准差为b =, 求t时刻目标的状态的最大似然估计。02正态分布概率密度函数1丄 f ifp (x) =e 21 b 丿b J 2兀假定观测为相互独立的0均值高斯白噪声。测量为r 0 的概率密度11函数为r20的概率密度函数为21-1P厂 f 1( X2)e * 21 b0 丿