褚東升，于興凱，張 玲

（中國海洋大學(xué)工程學(xué)院，山東省高校海洋機電裝備與儀器重點實驗室，山東 青島 266100）

目前國內(nèi)外的眾多學(xué)者，針對帶有約束條件的卡爾曼濾波算法做了大量的研究工作。運用系統(tǒng)特性到實際系統(tǒng)的等式關(guān)系，結(jié)合卡爾曼濾波算法Tahk［1］，Blair和Alouani尋找解決了目標(biāo)運動，Chen和Chiang［2］解決了信號失真等問題。Simon D［3］，Chia T L［4］提出狀態(tài)約束濾波算法和針對統(tǒng)計狀態(tài)約束條件出現(xiàn)的情況下的約束濾波算法。針對非線性系統(tǒng)Teixeira［5］，Simon D［3］提出了適應(yīng)于非線性系統(tǒng)的附有狀態(tài)等式約束條件的狀態(tài)估計算法。同時眾多學(xué)者給出許多應(yīng)用實例。目前處理等式約束卡爾曼濾波問題的主要方法可以歸納為以下幾種：偽測量法［1］、數(shù)學(xué)投影法［6］、減少模型參數(shù)化方法、兩步約束法［7］、再參數(shù)化法。

雖然對帶有等式約束動態(tài)濾波算法的研究進行了相當(dāng)長的時間。但帶有等式約束動態(tài)濾波算法大都建立在線性系統(tǒng)模型之上，而在實際系統(tǒng)中還往往存在著乘性噪聲的干擾，觀測方程往往本質(zhì)上是非線性的，而帶乘性噪聲隨機系統(tǒng)的控制和濾波問題研究已廣泛應(yīng)用于石油地震勘探、目標(biāo)識別、語音處理、水下目標(biāo)跟蹤等領(lǐng)域。針對帶乘性噪聲系統(tǒng)狀態(tài)附等式約束的濾波問題是一個新的研究方向。

本文針對乘性噪聲系統(tǒng)狀態(tài)附等式約束的最優(yōu)濾波算法進行研究。把狀態(tài)等式約束擴維到觀測方程中，然后基于投影定理對擴維后的系統(tǒng)進行最優(yōu)濾波求解。把求解得到的濾波算法各個量用無約束最優(yōu)濾波算法的各個量表示出來，得到的算法結(jié)果與基于數(shù)學(xué)最優(yōu)化理論求解得到的算法進行比較。同時對一個算例進行仿真，驗證該算法的有效性。

1 乘性噪聲無約束系統(tǒng)的狀態(tài)濾波算法

對于帶乘性噪聲的離散隨機無約束系統(tǒng)，其狀態(tài)空間方程表示為

其中：xk∈Rm為狀態(tài)向量；zk∈Rn為量測向量；wk∈Rr為系統(tǒng)噪聲；vk∈Rn為量測噪聲；mk為一維乘性噪聲；A，C為相應(yīng)維數(shù)的系數(shù)陣。設(shè)系統(tǒng)滿足以下條件：

D5：隨機序列 ｛mk｝，｛wk｝，｛vk｝及初始狀態(tài)x0相互統(tǒng)計獨立。

則有系統(tǒng)的狀態(tài)濾波公式［8］如下：

其中：Dk是s×m階常量矩陣；dk是s×1階常向量；s是狀態(tài)約束量，并且s≤m。本文假設(shè)Dk是滿秩矩陣，且秩為s階。

基于（1）、（2）、（9），系統(tǒng)可變形為

其中：xkD∈Rm為狀態(tài)向量；zkD∈Rn＋s為量測向量；wk∈Rr為過程噪聲；vkD∈Rn＋s為量測噪聲；mk為一維乘性噪聲；A，CD為相應(yīng)維數(shù)的系數(shù)陣。設(shè)該系統(tǒng)滿足以下條件：

D5：隨機序列mk，wk，vkD及初始狀態(tài)x0D相互統(tǒng)計獨立。

這里用上標(biāo)D來表示狀態(tài)附有等式約束的乘性噪聲系統(tǒng)各個量。不加上標(biāo)表示常量或變量與無約束系統(tǒng)中的量相同。

2 基于投影定理求解最優(yōu)濾波算法

對（10）、（11）構(gòu)成的系統(tǒng)用文獻［9］介紹的基于投影定理和擴維卡爾曼濾波方法，求得狀態(tài)最優(yōu)濾波公式如下：

3 算法比較

根據(jù)文獻［4，11］用基于數(shù)學(xué)最優(yōu)化理論的方法求解系統(tǒng)（1）、（2）、（9）的狀態(tài)濾波公式，該法是把無約束卡爾曼濾波映射到約束空間中。對于任意時刻k，用xkM/k表示狀態(tài)受約束的估計值，xk/k表示狀態(tài)沒有約束的狀態(tài)濾波值，Wk是任意對稱正定權(quán)重矩陣。

根據(jù)這種方法求解得到的狀態(tài)濾波值為：

根據(jù)誤差最小化可以得到Wk＝Pk－/k1 時得到的狀態(tài)濾波值最優(yōu)。即：

更新后估計誤差協(xié)方差可用下公式求得：

用文獻［4］方法求解可以看到有以下不足之處：

（1）每一步都需要求解無約束系統(tǒng)的狀態(tài)濾波值和估計誤差協(xié)方差，增加了計算量；

（2）只是基于數(shù)學(xué)最優(yōu)化方法求解狀態(tài)最優(yōu)濾波值，沒有去考慮整套濾波公式的實時遞推；

（3）沒有考慮到觀測方程對結(jié)果的影響。

下面對系統(tǒng)（10）（11）的狀態(tài)約束濾波公式結(jié)果用系統(tǒng)（1）（2）的無約束濾波公式各個量進行表示，尋找其中的聯(lián)系，并與數(shù)學(xué)最優(yōu)化算法的求解結(jié)果進行對比分析。假設(shè)2個系統(tǒng)k－1時刻（初始啟動時刻）的濾波值、誤差協(xié)方差、狀態(tài)Sk－1值相同，于是對比k時刻兩個系統(tǒng)濾波結(jié)果。

假設(shè)：

則可以得到：

再求新息：

同時：

根據(jù)文獻［12］，

由（16）、（17）、（22）、（23）可以求得：

定理1 本文基于投影定理求解的最優(yōu)濾波算法中增益量（16）與無約束濾波算法增益量（7）有如下關(guān)系

證畢。

由（13）、（22）、（24）、（25）可得：

定理2 本文基于投影定理求解的最優(yōu)濾波算法中濾波值（13）與無約束濾波算法濾波值（4）有如下關(guān)系：

證明

證畢。

定理3 本文基于投影定理求解的最優(yōu)濾波算法中誤差方差矩陣（15）與無約束濾波算法誤差方差矩陣（6）有如下關(guān)系

證明

由此可得：

證畢。

根據(jù)式（19）～（26）可以得到用本文介紹的方法求解的狀態(tài)最優(yōu)濾波與無約束狀態(tài)濾波公式的聯(lián)系。同時可以看到該法與數(shù)學(xué)最優(yōu)化方法求解結(jié)果［4，6，11］的不同之處。對比（18）與（25）可得到本文介紹方法得到的狀態(tài)最優(yōu)濾波值與文獻［4］方法的濾波值在結(jié)果上是等價的。不同之處在于式（23）、（24）、（26）表示的變量。

通過對比可以得到本文介紹的算法保證了增益、誤差協(xié)方差及狀態(tài)估計值的實時更新特性。同時避免了用無約束濾波值作為中間變量的麻煩，保證算法的簡潔精確。減小了計算量和計算誤差，提高了算法運行效率。兩種方法求得的狀態(tài)估計值都有以下兩個性質(zhì)［4，11］：

性質(zhì)1 根據(jù)系統(tǒng)（10）、（11）進行濾波得到狀態(tài)估計值（26）是狀態(tài)真實值的一個無偏估計。即

性質(zhì)2 約束卡爾曼濾波估計值的誤差協(xié)方差比無約束的卡爾曼濾波估計的誤差協(xié)方差更小。即

證明過程與參考文獻［4，11］推導(dǎo)類似。

4 算例分析

車輛運動系統(tǒng)的狀態(tài)方程和觀測方程給出如下［13］：

假設(shè)狀態(tài)向量x的前2個分量是地面車輛的北坐標(biāo)和東坐標(biāo)，后2個分量是北方向速度和東方向速度。由東逆時針測定航向角度θ，汽車沿著角度θ行駛。位置觀測設(shè)備給出了帶觀測噪聲的北方向和東方向位置值。

其中T表示離散步長，即歷遠間隔，設(shè)T＝1s。取θ＝π/3。過程噪聲協(xié)方差為Qk＝Diag（4，4，1，1），觀測噪聲協(xié)方差為Rk＝Diag（5，5）。乘性噪聲均值為＝2，方差為＝0.2。初始估計誤差協(xié)方差P0＝Diag（9，9，4，4）。根據(jù)實際情況，如果知道汽車行駛在航向角為θ的道路上，可以得到tanθ＝x1/x2＝x3/x4。由此可得到等式約束Dkxk＝dk。其中

利用Matlab分別對基于數(shù)學(xué)最優(yōu)化理論算法［4］和本文中基于投影定理的算法進行仿真，歷元總數(shù)為300s，北方向和東方向絕對誤差如圖所示。其中紅線代表本文算法絕對誤差，藍線代表數(shù)學(xué)最優(yōu)化理論算法絕對誤差。

通過圖1和2可以看出前2個狀態(tài)在2種算法下絕對誤差的大小，可以很直觀的看到本文算法優(yōu)于文獻［3］算法，說明了本文算法的有效性?？紤]到信噪信噪比（SNR）對濾波結(jié)果的影響。本文做了大量的仿真實驗，表一給出前2個狀態(tài)量在不同信噪比及不同算法下誤差的平均值大小。從表1中可以看出，信噪比越大，濾波誤差平均值越小，即濾波效果越好。同時通過對比可以直觀得到本文算法的有效性。

仿真中乘性噪聲解釋為觀測中存在的信號的延遲、信道的衰減及無關(guān)數(shù)據(jù)的干擾等復(fù)雜因素的綜合。以上仿真結(jié)果驗證了本文算法的有效性，通過多次仿真本文算法的高效性也可以體現(xiàn)。同時在仿真中也顯現(xiàn)出來了該方法的不足，在信噪比較?。ㄐ∮?0－2）時，仿真對比效果不明顯，所有算法效果都不好。對于信噪比很小的特殊情形有待進一步研究。

圖1 北方向位置絕對誤差Fig.1 The absolute error of North position

圖2 東方向位置絕對誤差Fig.2 The absolute error of East position

表1 不同信噪比濾波誤差均值的大小Table 1 Filtering average error average with different signal to noise ratio

5 結(jié)語

本文給出了在乘性噪聲系統(tǒng)中狀態(tài)附等式約束的一套最優(yōu)濾波算法。該算法是通過把狀態(tài)等式約束擴維到觀測向量中，利用投影定理對變形后的系統(tǒng)進行最優(yōu)濾波求解。進一步把得到的算法各個量用無約束濾波算法對應(yīng)各個量表示出來，并與無約束濾波算法和基于數(shù)學(xué)最優(yōu)化理論方法得到的算法進行比較分析驗證該算法的更新特性和簡潔有效性。實例仿真進一步驗證了該算法誤差小、效率高的特性。本文介紹的等式約束是線性的，對于帶乘性噪聲系統(tǒng)非線性的等式約束問題有待進一步研究。

［1］Tahk M，Speyer J L.Target tracking problems subject to kinematic constraints［J］.IEEE Tram Automat Cont，1990，35（3）：324-326.

［2］Chen Y H，Chiang C T.Adaptive beamforming using the constrained Kalman filter［J］.IEEE Transaction on Antennas and Propagation，1993，41（11）：1576-1580.

［3］Simon D，Chia T L.Kalman filtering with state equality constraints［J］.IEEE Transactions on Aerospace and Electronic Systems，2002，38（1）：128-136.

［4］Chia T L，Simon D，Chizeck H J.Kalman filtering with statistical state constraints［J］.Control and Intelligent Systems，2006，34（1）：73-79.

［5］Teixeira B O S.State Estimation for Equality-Constrained Linear Systems［C］.LA：Proceeding of the 46th IEEE Conference on Decision and Control New Orleans，2007：12-14.

［6］Simon D.Kalman filtering with state constraints：a survey of linear and nonlinear algorithms［J］.IET Control Theory and Applications，2010，4（8）：1303-1318.

［7］Simon J，Julier Joseph J，LaViola Jr.On Kalman filtering with nonlinear equality constraints ［J］.IEEE Transaction on Signal Processing，2007，55（6）：2774-2784.

［8］褚東升，張玲.通道相關(guān)時帶乘性噪聲系統(tǒng)最優(yōu)濾波 ［J］.青島海洋大學(xué)學(xué)報，2002，32（6）：987-992.

［9］宋文堯，張牙.卡爾曼濾波［M］.北京：科學(xué)出版社，1991.

［10］Benzi M，Golub G H，Liesen J.Numerical solution of saddle point problems［J］.Acta Numerica，2005，14（1）：1-137.

［11］Simon D.Kalman filtering with state constraints［J］.IET Control Theory Appl，2010，4（8）：1303-1318.

［12］Gupta N，Hauser R，Johnson N F.Forecasting Financial Time-Series using Artificial Market Models［C］.Oxford na-tr：Proceedings of the 10th Annual Workshop on Economic Heterogeneous Interacting，2005，05/09.

［13］宋迎春.動態(tài)定位中的卡爾曼濾波研究 ［D］.長沙：中南大學(xué)，2006.

［14］Chun Yang，Erik Blasch.Kalman filtering with nonlinear state constraints［J］.IEEE Transactions On Aerospace and Electronic Systems J，2009，45（1）：70-84.