第二章-狀態(tài)估計基礎

1、第二章狀態(tài)估計基礎狀態(tài)估計的目的是對目標過去的狀態(tài)進行平滑、對目標現(xiàn)在的運動狀態(tài)進行濾波和對目標未來的運動狀態(tài)進行預測。這些運動狀態(tài)包括目標位置、速度、加速度等。本章討論在多傳感器跟蹤系統(tǒng)中廣泛應用的狀態(tài)估計技術，這些技術包括Kalman濾波技術，濾波與濾波技術、最小二乘濾波技術和非線性系統(tǒng)的狀態(tài)估計技術。2.1 線性系統(tǒng)估計卡爾曼濾波技術2.1.1 線性系統(tǒng)描述1.離散時間線性動態(tài)系統(tǒng)的狀態(tài)方程線性系統(tǒng)采用狀態(tài)方程、觀測方程及其初始條件來描述。線性離散時間系統(tǒng)的一般狀態(tài)方程可描述為其中，是時刻目標的狀態(tài)向量， 是過程噪聲，它是具有均值為零、方差矩陣為的高斯噪聲向量，即（：狄拉克函數(shù)，或單位沖

2、激函數(shù)），是狀態(tài)轉移矩陣，是過程。2. 傳感器的測量（觀測）方程傳感器的通用觀測方程為這里，是傳感器在時刻的觀測向量，觀測噪聲是具有零均值和正定協(xié)方差矩陣的高斯分布測量噪聲向量，即3初始狀態(tài)的描述初始狀態(tài)是高斯的，具有均值和協(xié)方差，即以上描述比較抽象，下面結合具體的例子加以說明。例1：目標沿軸作勻速直線運動，過程噪聲為速度噪聲，試寫出目標的狀態(tài)方程。解：目標的狀態(tài)為用表示時間間隔，表速度噪聲，則有寫成矩陣形式為令有其中：均值為0，方差為的高斯噪聲。例2：目標為二維空間中作勻速直線運動的目標，過程噪聲為加速度噪聲，試寫出目標的狀態(tài)方程。解：由于目標為二維空間作勻速直線運動的目標，目標的狀態(tài)中有目

3、標的位置和目標的速度，那么目標的狀態(tài)可寫為用表示時間間隔，分別表示方向的加速度噪聲，則有寫成矩陣的形式有令， ，有假定為均值為0，方差分別為和的相互獨立的高斯白噪聲，則例3：目標為沿軸做勻加速運動的目標，過程噪聲為加速度噪聲，試寫出目標的狀態(tài)方程解：目標的狀態(tài)可寫為：用表示時間間隔，分別表示方向的加速度噪聲，則有寫成矩陣的形式有令有其中，為方向加速度噪聲的方差。例4：在例3的基礎上，假定目標為三維空間中作勻加速運動的目標，過程噪聲為加速度噪聲，試寫出目標的狀態(tài)方程。解：由于目標為三維空間作勻速直線運動的目標，目標的狀態(tài)中有目標的位置和目標的速度，那么目標的狀態(tài)可寫為其中目標的狀態(tài)方程可寫為其中

4、而過程噪聲協(xié)方差矩陣為分別為方向，方向和方向加速度噪聲方差。例5：假定對二維空間作勻速直線運動的目標進行觀測時，觀測值為目標的位置加上觀測噪聲，試寫出目標的觀測方程。解：由前面可知，二維空間中作勻速直線運動的目標，其狀態(tài)向量為由題意得目標的觀測方程為其中分別為和方向的觀測噪聲。將上式寫成矩陣的形式，有令，則有假定為均值為零，方差分別為的高斯白噪聲，則例6：假定對三維空間作勻速直線運動的目標進行觀測時，觀測值為目標的位置加上觀測噪聲，試寫出目標的觀測方程。解：由前面可知，三維空間中作勻速直線運動的目標，其狀態(tài)向量為由題意得目標的觀測方程為其中分別為和方向的觀測噪聲。將上式寫成矩陣的形式，有有假定

5、為均值為零，方差分別為的高斯白噪聲，則作業(yè)：假定對三維空間作勻加速運動的目標進行觀測時，觀測值為目標的位置加上觀測噪聲，試寫出目標的觀測方程。例7：設目標沿軸勻速直線運動，目標的狀態(tài)可表示為，在時刻的觀測值為，在時刻的觀測值為，采樣間隔為，求目標的初始狀態(tài)和初始協(xié)方差。解：初始狀態(tài)為初始協(xié)方差其中，為觀測噪聲的方差，即：,濾波器從時開始工作。2.1.2 Kalman濾波算法狀態(tài)估計的一步預測方程為一步預測的協(xié)方差為預測的觀測向量為觀測向量的預測誤差協(xié)方差為新息或量測殘差為濾波器增益為Kalman濾波算法的狀態(tài)更新方程為濾波誤差協(xié)方差的更新方程為2.2轉換坐標卡爾曼濾波器（非線性估計技術）?？?/p>

6、曼濾波器兩個要求：1) 目標的狀態(tài)方程是線性的；2）觀測方程是線性的。在實際的情況下，要同時滿足這兩個要求是困難的。通常情況下：狀態(tài)方程在直角坐標系下是線性的，而觀測方程是在極坐標系下獲得的關于目標的測量。如雷達的測量是距離、方位角和高低角。從直角坐標系來看，觀測方程是非線性的。假定雷達的測量為距離、方位角和高低角，測量與目標位置的關系為這是一個非線性關系。為解決測量方程非線性情況下的目標跟蹤問題，可采用兩種方法，一種是采用擴展的卡爾曼跟蹤濾波器，這將在后面介紹；另一種是對測量進行坐標轉換，將極坐標下的測量轉化為直角坐標系下的測量，然后用標準的卡爾曼濾波器進行跟蹤。對三坐標雷達，坐標轉換公式為

7、噪聲方差的轉換公式為 假設距離、方位角和高低角的測量噪聲為相互獨立的高斯白噪聲，即并且對兩坐標雷達，極坐標系下的觀測為（），轉換成直角坐標為 方差的轉換：為求方差的轉換，先求偏導于是，得在得到直角坐標系下的測量及測量的協(xié)方差矩陣后，可以用前面講的方法進行目標跟蹤。這種變換存在的問題是：在直角坐標系下，測量的噪聲不再是嚴格意義上的高斯噪聲，并且噪聲的分布中心并不是以直角坐標為中心，而且在直角坐標系下噪聲是相互關聯(lián)的。 由于噪聲的分布中心并不是以直角坐標為中心，因此，噪聲是有偏的，為此，有些學者提出了無偏坐標轉換的方法：對轉換后的直角坐標和方差進行校正。通常情況下，傳感器的測量為極坐標測量，極角坐

8、標測量可通過無偏坐標轉換變?yōu)橹苯亲鴺藴y量。設，則無偏轉換的測量為 其中，。轉換測量的協(xié)方差為 其中在動態(tài)方程為線性、觀測方程為非線性的情況下，可使用轉換坐標卡爾曼濾波器，轉換可采用兩種方法：一種是不帶校正量的轉換，這是一種相對簡單的方法；另一種為帶校正量的方法，這是一種相對復雜的方法。2.3擴展卡爾曼濾波器(非線性估計技術)采用擴展的卡爾曼濾波器進行目標跟蹤。2.3.1 系統(tǒng)的狀態(tài)方程和測量方程狀態(tài)方程：其中為狀態(tài)轉移矩陣。測量方程：其中2.3.2、觀測方程線性化將等號后的第一項以為中心按泰勒級數(shù)展開并略去二級以上的高階分量，有代入上式，得令，于是得到線性化的觀測方程為假定目標的狀態(tài)為,而，則

9、為的雅可比矩陣(Jacobian矩陣)。例：雷達的觀測是在極坐標系下進行的，對于一個直角坐標為的目標，雷達所測的三個極坐標分別為觀測方程為令目標的狀態(tài)為，則線性化的觀測矩陣為其中例：紅外傳感器,觀測值為方位角和高低角 ，求目標的狀態(tài)為時線性化的觀測矩陣解：因2.3.3、擴展卡爾曼濾波方程一步預測：一步預測協(xié)方差矩陣預測的觀測向量為 觀測矩陣新息或量測殘差為殘差協(xié)方差矩陣 濾波器增益 濾波輸出誤差的協(xié)方差陣例題2-2：利用擴展的卡爾曼濾波算法對y軸上勻速運動的目標進行跟蹤。設傳感器平臺在x-y平面內運動，運動方程為式中，t為時間，和為相互獨立的、零均值白色高斯噪聲，其方差分別為，且與過程噪聲和量

10、測噪聲相互獨立。過程噪聲為加速度噪聲，其方差為，傳感器測量目標的方位角，采樣時間間隔，測量噪聲為零均值的高斯噪聲，其方差為。解：目標在y軸上運動，系統(tǒng)的狀態(tài)方程為其中，傳感器的測量方程為其中，建立目標位置估計值和方位角測量值之間的關系(不考慮測量誤差)為所以, 由目標的初始狀態(tài)對上式求導可得于是初始狀態(tài)的方差線性化的觀測矩陣2.4 無跡卡爾曼濾波器（Unscented Kalman Filter, UKF）目前，擴展卡爾曼濾波雖然被廣泛用于解決非線性系統(tǒng)狀態(tài)估計問題，但其濾波效果在很多復雜的系統(tǒng)中并不能令人滿意。模型的線性化誤差往往會嚴重影響最終的濾波精度，甚至導致濾波器發(fā)散。另外，在許多實際

11、的應用中，模型的線性化過程比較繁雜，而且也不易得到。無跡卡爾曼濾波是由于不需要對非線性系統(tǒng)進行線性化，并可以很容易地應用于非線性系統(tǒng)的狀態(tài)估計，因此，UKF方法在許多方面得到應用。2.4.1 無跡變換無跡卡爾曼濾波是在無跡變換的基礎上發(fā)展起來的。無跡變換（Unscented Transformation, UT）的基本思想是由Julier等首先提出的，是用于計算經(jīng)過非線性變換的隨機變量統(tǒng)計的一種新方法。它不需要對非線性狀態(tài)和測量方程模型進行線性化。假定X為一個維的隨機變量，為一非線性函數(shù)，并且。的均值和協(xié)方差分別為和。無跡變換的一般步驟如下：（1） 確定()個采樣點其中，為一尺度參數(shù)，可以為的

12、任意值。為的第i列。為狀態(tài)向量X的維數(shù)。（2） 每個采樣點通過非線性函數(shù)傳播，得到；（3）的估計值和協(xié)方差估計如下2.4.2 無跡Kalman濾波算法根據(jù)無跡變換的特點，Kalman濾波算法可歸納如下：假定狀態(tài)方程：觀測方程：k時刻的狀態(tài)估計值和狀態(tài)協(xié)方差：和k時刻的觀測向量（1） 根據(jù)采樣規(guī)則，確定2+1個采樣點以及相應的加權值其中，為尺度參數(shù)，可以為的任意值。為均方根的第i列。為狀態(tài)向量的維數(shù)。 （2） 一步預測； ； （3） 更新狀態(tài)在動態(tài)方程和觀測方程同時為非線性、或者一個為線性另一個為非線性的情況下，可以使用無跡卡爾濾波器，無跡卡爾曼濾波器的缺點是計算量較大。與無跡卡爾濾波器類似的濾

13、波器還有積分卡爾濾波器（QKF）和粒子濾波器，這些濾波器均可用于非線性系統(tǒng)，其缺點是計算量更大。2.5 和濾波器和濾波器分別用于對勻速和勻加速目標進行跟蹤。2.5.1、濾波器濾波器用于對勻速運動的目標進行跟蹤。目標的狀態(tài)方程為式中，目標的觀測方程式中，濾波方程問題：如何求和，利用穩(wěn)態(tài)濾波器的特點，即得其中，為目標的機動系數(shù)。狀態(tài)估計狀態(tài)估計的協(xié)方差為一步預測協(xié)方差為以上只討論的是沿軸作直線運動情況下的濾波，對于平面或空間目標，需要分別對、及上的狀態(tài)向量分別濾波。當過程噪聲方差不能事先確定時，目標的機動系數(shù)無法確定，和兩參數(shù)無法確定。工程上經(jīng)常采用兩種的方法來確定和：1、常系數(shù)法：和取固定的值一

14、般取(1) (臨界阻尼法)(2) (最佳選擇法)2、變系數(shù)法：這里從1開始計數(shù)。對來說時才有值，但濾波器從開始工作，前兩個點用于確定目標的初始位置和速度，完成航跡起始。2.5.2 濾波器濾波器用于對勻加速運動的目標進行跟蹤。目標的狀態(tài)方程為式中，目標的觀測方程式中，濾波方程問題：如何求、和，利用穩(wěn)態(tài)濾波器的特點，即得三個非線性方程構成的方程組 其中，為目標的機動系數(shù)。解線性方程組，得、和。狀態(tài)估計狀態(tài)估計的協(xié)方差為以上只討論的是沿軸作直線運動情況下的濾波，對于平面或空間目標，需要分別對、及上的狀態(tài)向量分別濾波。當過程噪聲方差不能事先確定時，目標的機動系數(shù)無法確定，、和三參數(shù)無法確定。工程上經(jīng)常

15、采用兩種的方法來確定、和1、常系數(shù)法：和取固定的值一般取2、變系數(shù)法：這里從1開始計數(shù)。對來說時才有值，對于來說時才有值，但濾波器從開始工作，前三個點用于確定目標的初始位置、速度和加速度，完成航跡起始。2.6 最小二乘估計和最小二乘濾波器2.6.1、線性最小二乘估計模型：其中， ；目標：求以使方法：得到估計：估計誤差的方差：其中例：設目標作勻速直線運動，目標的狀態(tài)可表示為，目標的起始狀態(tài)為，目標的起始時刻為，在時刻的觀測值為，在時刻的觀測值為，觀測噪聲為相互獨立的高斯白噪聲，分別求和時刻目標狀態(tài)的最小二乘估計。a、求時刻目標狀態(tài)的估計值解：得到估計：估計誤差的方差：其中b、求時刻目標狀態(tài)的估計

16、值得到估計：估計誤差的方差：其中2.6.2、加權最小二乘估計 當測量數(shù)據(jù)的測量精度不等時，應采用加權處理，對精度較高的測量結果賦以較大的權。目標：求以使得到估計：估計誤差的方差：其中2.6.3、馬爾可夫估計馬爾可夫估計是一切加權最小二乘估計中能使得估計方差陣達到最小的一種。當取時，加權最小二乘估計為馬爾可夫估計估計值為：其估計誤差的方差為2.6.4、非線性最小二乘估計模型：目標：求以使另一種表示加權最小二乘估計目標：令，非線性加權最小二乘簡化為非線性最小二乘。非線性最小二乘估計可以用Gauss-Newton算法或Levenberg-Marquart算法（這些算法在Matlab中都已編好，直接應

17、用就行，如lsqnonlin，當然也可自行編寫）求解。這里給出一種自我實現(xiàn)的方法。令則Levenberg-Marquart算法求解非線性最小二乘的迭代公式為 令一種偽語言描述的LM算法如下：;e1:=10-15;e2:=10-15; e3:=10-15;kmax=100;k:=0;v:=2;p=X0;A:=JT*J;Ep:=Y-F(p);g:=JT*Ep; Stop:=(); ;while(not stop)and(kkmax)k:=k+1; repeat Solve (A+I)*=gif () stop:=true;elsepnew:=p+;if p:=pnew;A:=JT*J;Ep:=Y-

18、F(p);g=:JT*Ep; Stop:=() or ;v:=2;elseendifendifuntil()or(stop)endwhileX:=p;非線性最小二乘估計也是一種最大似然估計，是一種最優(yōu)估計，估計誤差的方差能達到CRLB，因此估計誤差的方差可取為CRLB。估計誤差的方差為例：勻速運動目標，目標的狀態(tài)為，時刻的測量為，時刻的測量為，傳感器測距誤差標準差為=10m,測角誤差標準差為=0.001rad, 求時刻目標的狀態(tài)。設時刻為，則時刻目標的狀態(tài)為時刻的理論觀測值為時刻的理論觀測值為加權非線性最小二乘估計的目標函數(shù)為時刻目標狀態(tài)的估計為Matlab求Jacobian矩陣的方法syms

19、 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(x2+y2)/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維雷達，測量目標的距離和方位角，傳感器位置為，其測距誤差標準差為100 m,測角誤差標準差為0.001弧度。在時，觀測值為4

20、9999.7, 1.，在時，觀測值為51111.3, 1.，在時，觀測值為52043.8, 1.，分為用線性最小二乘和非線性最小二乘法估計目標的初始狀態(tài)和狀態(tài)估計的協(xié)方差。2.6.5、最大似然和非線性最小二乘的轉化例：勻速運動目標，目標的狀態(tài)為，時刻的測量為，時刻的測量為，傳感器測距誤差標準差為=10m,測角誤差標準差為=0.005rad, 求時刻目標的狀態(tài)的最大似然估計。正態(tài)分布概率密度函數(shù)假定觀測為相互獨立的0均值高斯白噪聲。測量為的概率密度函數(shù)為的概率密度函數(shù)為測量為和測量同時出現(xiàn)的聯(lián)合概率密度函數(shù)（似然函數(shù)）為對數(shù)似然2.6.6、最小二乘濾波器模型：狀態(tài)方程觀測方程求按最小二乘估計的方

21、法，先確定與間的關系得到最小二乘估計其中， ；取加權矩陣，作用是使過老數(shù)據(jù)的作用逐漸消失。取則加權最小二乘估計為其中現(xiàn)在假設知道，求，于是就形成了最小二乘濾波器。加權最小二乘濾波器的遞推公式為 其中，。例：設目標為勻速運動模型，即狀態(tài)向量為觀測為目標在直角坐標下的位置，寫出最小二乘濾波器中的狀態(tài)轉移矩陣及觀測矩陣。解：轉移矩陣為其中觀測矩陣為時刻的觀測例2-4 目標為二維空間中作勻速直線運動的目標，其初始狀態(tài)為, 過程噪聲的方差q=1。傳感器為2維雷達，測量目標的距離和方位角，傳感器位置為，其測距誤差標準差為100 m,測角誤差標準差為0.001弧度，采樣時間間隔為10 s。試：1、根據(jù)已知條件產(chǎn)生仿真的觀測數(shù)據(jù)。2、利用仿真的觀測數(shù)據(jù)分別用變系數(shù)濾波器、轉換坐標Kalman濾波器、最小二乘濾波器和擴展Kalman濾波器進行目標跟蹤，并在同一圖上繪制出跟蹤結果。2.7 空間配準和坐標變換在對坐標位置的不同傳感器所獲得的數(shù)據(jù)進行處理時，首先要作的是將測量數(shù)據(jù)轉換到公共的坐標系下，然后才能對數(shù)據(jù)進行處理。通常的坐標系有以下4種：（1） 以本傳感器位置為中