基于卡爾曼濾波的電網(wǎng)頻率檢測(cè)測(cè)與預(yù)測(cè)

1、華北電力大學(xué)本科畢業(yè)設(shè)計(jì)（論文）摘 要頻率偏移嚴(yán)重影響電力系統(tǒng)運(yùn)行的可靠性，對(duì)電力用戶經(jīng)濟(jì)性與安全性產(chǎn)生危害。頻率偏移嚴(yán)重時(shí)，還可能導(dǎo)致電力系統(tǒng)瓦解，造成重大供電事故。故頻率穩(wěn)定是系統(tǒng)安全、經(jīng)濟(jì)運(yùn)行的重要指標(biāo)。電力系統(tǒng)頻率是檢驗(yàn)電能生產(chǎn)質(zhì)量的指標(biāo)之一，也是衡量電力系統(tǒng)運(yùn)行狀態(tài)的重要參數(shù)。一般情況下，系統(tǒng)頻率反映了電力系統(tǒng)中有功功率供需平衡的基本狀態(tài)，它將隨負(fù)荷波動(dòng)在小范圍內(nèi)緩慢變化。在穩(wěn)定的運(yùn)行狀態(tài)中，發(fā)電機(jī)輸出功率與系統(tǒng)負(fù)荷及損耗維持平衡，但大容量負(fù)荷或發(fā)電機(jī)的投切以及控制設(shè)備的不完善都可能導(dǎo)致頻率偏移，給電力系統(tǒng)的穩(wěn)定運(yùn)行和用戶設(shè)備的正常工作帶來影響，如不采取有效措施，將導(dǎo)致機(jī)組損壞、系統(tǒng)

2、瓦解的重大惡性事故，造成巨大經(jīng)濟(jì)損失。因而電力系統(tǒng)頻率檢測(cè)和控制是系統(tǒng)運(yùn)行的重要任務(wù)之一。本課題主要是采用基于卡爾曼濾波原理，設(shè)計(jì)出對(duì)電網(wǎng)頻率進(jìn)行綜合檢測(cè)和預(yù)測(cè)的方案來對(duì)電網(wǎng)進(jìn)行頻率檢測(cè)和預(yù)測(cè)，最后通過實(shí)例驗(yàn)證證明了該方案的可行性。這對(duì)于提高電力系統(tǒng)運(yùn)行的穩(wěn)定性具有重要的作用。關(guān)鍵詞：頻率偏移，檢測(cè)，控制，卡爾曼濾波AbstractFrequency offset seriously affect the reliability of the power system operation, it is harm to power users and security of economic .

3、When the frequency offset serious, may also lead to the collapse of the power system, resulting in significant power accident. Therefore, the frequency stability of system security is important to indicator of economic performance. Power system frequency is not only one of the indicators of the qual

4、ity inspection of power production, but also an important parameter to measure the state of the power system operation. General, the system reflects the power system frequency active power supply and demand balance in the basic state; it will slowly change with load fluctuations within a small range

5、. In steady state operation, the generator output power and the system load and the loss are maintaining a balance. However, switching of the generator and control equipment imperfect or large capacity load may lead to frequency offset, and it will impact the normal operation and stable operation of

6、 the power system, it will result in damage to the unit, major fatal accidents system collapse when no measures were taken, and it will cause huge economic losses. Thus the power system frequency detection and control is one of the important tasks of system operation. The main subject is based on th

7、e principle of Kalman filter, designed to be integrated grid frequency detection and prediction programs to detect and predict the frequency on the grid. finally，an example was verified to demonstrate the feasibility of the scheme. It is important to improve the operation stability of the power syst

8、em.Keywords：Frequency offset, Detection, Control, Kalman filteII目 錄摘 要IAbstractII第一章 緒論21.1 課題研究的背景和意義21.2 國(guó)內(nèi)外發(fā)展現(xiàn)狀3第二章 卡爾曼濾波算法的結(jié)構(gòu)特點(diǎn)52.1 卡爾曼濾波的基本思想52.2.1 擴(kuò)張卡爾曼濾波62.2.2 無跡卡爾曼濾波82.3 卡爾曼濾波器的應(yīng)用10第三章 時(shí)變隨機(jī)信號(hào)及其測(cè)量過程數(shù)學(xué)模型的建立113.1 隨機(jī)信號(hào)簡(jiǎn)介113.2 一維時(shí)變隨機(jī)信號(hào)的數(shù)學(xué)模型113.3 信號(hào)測(cè)量過程的數(shù)學(xué)模型12第四章 向量卡爾曼濾波和預(yù)測(cè)的一般方法144.1 標(biāo)量卡爾曼濾波器的基本內(nèi)

9、容144.2 最優(yōu)遞歸型估計(jì)器的構(gòu)成164.3 標(biāo)量卡爾曼濾波器的遞推算法174.4 標(biāo)量卡爾曼預(yù)測(cè)器184.5 向量卡爾曼濾波和預(yù)測(cè)20第五章 基于卡爾曼濾波原理對(duì)電網(wǎng)頻率進(jìn)行檢測(cè)和預(yù)測(cè)255.1 電力系統(tǒng)狀態(tài)空間模型255.2 基于擴(kuò)展卡爾曼濾波的估計(jì)實(shí)現(xiàn)275.3 數(shù)字仿真29總 結(jié)30致 謝31參考文獻(xiàn)32附錄34第一章 緒論 1.1 課題研究的背景和意義隨著電力電子技術(shù)的發(fā)展，電力電子裝置帶來的頻率擾動(dòng)對(duì)電力系統(tǒng)安全、穩(wěn)定、經(jīng)濟(jì)運(yùn)行構(gòu)成潛在威脅，給周圍電氣環(huán)境帶來了極大影響，同時(shí)也阻礙了電力電子技術(shù)的發(fā)展1。因此，對(duì)電力系統(tǒng)頻率問題的研究已被人們逐漸重視。電力電子裝置等非線性負(fù)載所產(chǎn)

10、生的諧波會(huì)引起負(fù)載和輸電設(shè)備的過載、失控和增加損耗，甚至嚴(yán)重危害電網(wǎng)和用電設(shè)備的安全。隨著電力電子技術(shù)在家庭、工業(yè)、交通、國(guó)防日益廣泛的應(yīng)用，電力電子裝置本身功率容量和功率密度的不斷增大，電網(wǎng)頻率不穩(wěn)定也日益嚴(yán)重。 頻率不穩(wěn)定危害可以歸結(jié)為：l)消耗無功與增加線路損耗； 2)引起設(shè)備過載、降低設(shè)備絕緣等級(jí)、加速絕緣老化甚至引起火災(zāi)等； 3)降低負(fù)載工作性能(如使電機(jī)產(chǎn)生附加力矩等)； 4)影響計(jì)量準(zhǔn)確度，影響繼電保護(hù)等裝置可靠運(yùn)行；5)對(duì)通信系統(tǒng)和計(jì)算機(jī)網(wǎng)絡(luò)產(chǎn)生電磁干擾(EMI)等；6)引起系統(tǒng)不穩(wěn)定，危害電網(wǎng)安全運(yùn)行。 電網(wǎng)頻率不穩(wěn)定已成為許多電子設(shè)備與系統(tǒng)現(xiàn)場(chǎng)可靠運(yùn)行的主要障礙之一，而且還

11、嚴(yán)重阻礙了諸如變頻調(diào)速等大批高效、節(jié)能電力電子技術(shù)的推廣應(yīng)用。因此，國(guó)內(nèi)外都在加緊研究頻率不穩(wěn)定的治理方法2。近些年，我國(guó)也開發(fā)了一些電力電子頻率實(shí)時(shí)跟蹤裝置，但在功能上、實(shí)用化方面還不夠理想，還存在許多問題： 1)處理功能較差、可擴(kuò)展存儲(chǔ)空間較小、運(yùn)算速度較慢，難以運(yùn)用精確嚴(yán)格的算法進(jìn)行大量的實(shí)時(shí)數(shù)據(jù)處理，不滿足電力監(jiān)測(cè)實(shí)時(shí)性的要求； 2)電力系統(tǒng)中最常用微處理器包括51系列和96系列等控制型器件，但隨著電力系統(tǒng)對(duì)實(shí)時(shí)性、數(shù)據(jù)量和計(jì)算要求的不斷提高，這些器件在計(jì)算能力方面已不能很好地適應(yīng)電力系統(tǒng)的要求，致使電力系統(tǒng)的高精度測(cè)量、實(shí)時(shí)監(jiān)控和先進(jìn)算法的運(yùn)用受到了限制。3)有的產(chǎn)品雖然直接引進(jìn)了國(guó)

12、外的技術(shù)模塊，功能較強(qiáng)，可是價(jià)格較高，且不完全適合我國(guó)市場(chǎng)； 4)有的產(chǎn)品無通訊和控制輸出功能，不滿足電力系統(tǒng)網(wǎng)絡(luò)化、自動(dòng)化的發(fā)展方向；頻率是電力系統(tǒng)是否穩(wěn)定的重要標(biāo)志之一，本系統(tǒng)主要針對(duì)電力系統(tǒng)中頻率進(jìn)行測(cè)量，實(shí)時(shí)跟蹤電網(wǎng)中頻率的波動(dòng)及變化，保證電力系統(tǒng)供電穩(wěn)定及改善國(guó)家電網(wǎng)中電能質(zhì)量。因此，有必要對(duì)電力系統(tǒng)頻率檢測(cè)與控制的研究現(xiàn)狀作較為全面的評(píng)述，為后續(xù)研究提供參考。1.2 國(guó)內(nèi)外發(fā)展現(xiàn)狀頻率的正確檢測(cè)是頻率控制的前提。頻率檢測(cè)一般可分為硬件檢測(cè)和軟件檢測(cè)兩類。硬件檢測(cè)3-4主要利用過零比較器或鎖相環(huán)實(shí)現(xiàn)。檢測(cè)方法幾乎不占用處理器時(shí)間，但是需要增加硬件偵測(cè)電路， 加大了開發(fā)成本和系統(tǒng)體積。

13、同時(shí)，檢測(cè)結(jié)果還易受諧波和器件零點(diǎn)漂移影響。軟件檢測(cè)5是通過某種算法對(duì)采樣信號(hào)進(jìn)行分析，得出頻率信息。軟件檢測(cè)只占用處理器一定時(shí)間，無需硬件電路，可節(jié)約成本。近年來國(guó)內(nèi)外學(xué)者提出了多種電氣信號(hào)的軟件頻率檢測(cè)算法， 分類歸納主要有以下幾種。1）基于正弦信號(hào)模型的檢測(cè)算法 對(duì)信號(hào)觀測(cè)模型進(jìn)行數(shù)學(xué)變換，將待測(cè)量f或f表示為樣本值的顯函數(shù)來估計(jì)，根據(jù)正弦函數(shù)的特性，從若干個(gè)采樣值中計(jì)算電氣信號(hào)頻率，如最大值算法、采樣值積算法、采樣值累計(jì)算法、三點(diǎn)頻率檢測(cè)法、Mann-Morrison導(dǎo)數(shù)算法和Prodar-70二階導(dǎo)數(shù)算法等6。這類算法的優(yōu)點(diǎn)是原理簡(jiǎn)單，信號(hào)觀測(cè)時(shí)間短，采樣點(diǎn)數(shù)較少，易于實(shí)現(xiàn)，響應(yīng)速度

14、較快。但這類算法難以考慮諧波、非周期分量和噪聲影響，且算法推導(dǎo)有近似化過程，精度總體不高，尤其是在非穩(wěn)態(tài)下頻率測(cè)量誤差較大，適應(yīng)于測(cè)量精度要求不高的應(yīng)用場(chǎng)合。2） 傅里葉變換檢測(cè)算法傅里葉變換檢測(cè)算法最常用的是離散傅里葉變換DFT或快速傅里葉變換算法FFT及它們的改進(jìn)算法。DFT(FFT)是一種典型的數(shù)字濾波技術(shù)，也是頻譜分析的主要工具，目前在電氣信號(hào)頻率測(cè)量乃至電參量檢測(cè)領(lǐng)域應(yīng)用最為廣泛。DFT(FFT)分離信號(hào)的基波及各次諧波分量，從而得到信號(hào)的電力系統(tǒng)頻率，即使頻率稍微偏移標(biāo)稱值，選擇合適的采樣技術(shù)和數(shù)據(jù)窗，利用前后窗DFT(FFT)結(jié)果能夠準(zhǔn)確地估計(jì)系統(tǒng)的頻率，精度和穩(wěn)定度好，計(jì)算量較

15、小。3) 過零檢測(cè)法 過零檢測(cè)法（Zero-crossing Algorithm）一般采用簡(jiǎn)單信號(hào)觀測(cè)模型，通過測(cè)量信號(hào)波形相繼過零點(diǎn)間的時(shí)間寬度來計(jì)算頻率。該方法物理概念清晰、計(jì)算量小，易受諧波、隨機(jī)干擾和非周期分量的影響，精度低，實(shí)時(shí)性不好，實(shí)用的測(cè)頻裝置很少單一地應(yīng)用過零檢測(cè)法算法。對(duì)它的改進(jìn)主要是提高其測(cè)量精度和實(shí)時(shí)性，典型的改進(jìn)算法有水平交（Level crossing）算法7、高次修正函數(shù)法8和最小二乘多項(xiàng)式曲線擬合法， 它們以計(jì)算量和復(fù)雜度為代價(jià)來提高算法的測(cè)量精度和響應(yīng)速度，一定程度上喪失了原始的周期法的簡(jiǎn)明性。 4) 隨即模型算法 a) 最小二乘算法（Least Error-

16、Square-algorithm）。最小二乘算法檢測(cè)頻率的基本原理是在最小方差意義下實(shí)現(xiàn)樣本數(shù)據(jù)與模型的最佳擬合，即對(duì)量測(cè)矩陣方程在極小化誤差向量加權(quán)二次范數(shù)的約束下利用觀測(cè)值求解待測(cè)量。 b) 最小絕對(duì)值近似（Least Absolute Value Approximation）。最小絕對(duì)值近似方法以極小化誤差向量的一次范數(shù)，這是它與LES算法的不同之處。LAV算法較LES算法在采樣率、數(shù)據(jù)窗長(zhǎng)度位置及壞數(shù)據(jù)對(duì)測(cè)量精度的影響等方面有所改善，但計(jì)算量大得多。 c) 牛頓類算法（Newton-type Algorithm）。牛頓類算法基本原理是將牛頓類迭代算法和最小二乘原理結(jié)合起來求解超定非線性

17、方程組。該算法主要用來測(cè)量系統(tǒng)諧波，由于要多次迭代，計(jì)算量很大， 尤其是在待估高次諧波分量較多時(shí)，工作量十分可觀；且算法受限于參數(shù)設(shè)置和初始值，容易出現(xiàn)數(shù)值不穩(wěn)定現(xiàn)象。 d) 離散卡爾曼濾波算法。 離散(擴(kuò)展)卡爾曼濾波法通過對(duì)離散隨機(jī)動(dòng)態(tài)過程及其含噪聲量測(cè)變換計(jì)算信號(hào)頻率，將卡爾曼濾波算法應(yīng)用于電力系統(tǒng)頻率測(cè)量的前提建立在模型（如動(dòng)態(tài)方程、量測(cè)方程和隨機(jī)序列的統(tǒng)計(jì)特性）和狀態(tài)變量、協(xié)方差陣初值的正確估計(jì)基礎(chǔ)上的。第二章 卡爾曼濾波算法的結(jié)構(gòu)特點(diǎn)2.1 卡爾曼濾波的基本思想卡爾曼濾波是線性無偏最小均方誤差遞推濾波器。與維納濾波相比，在平穩(wěn)條件下，它們所得到的穩(wěn)態(tài)結(jié)果是一致的。然而，它們解決的方

18、法有很大區(qū)別。維納濾波是根據(jù)全部過去的和當(dāng)前的觀察數(shù)據(jù)來估計(jì)信號(hào)的當(dāng)前值，它的解是以均方誤差最小條件下所得到的系統(tǒng)的傳遞函數(shù)H(z)或單位樣本響應(yīng)h(n)的形式給出的，因此稱這種系統(tǒng)為最佳線性過濾器或?yàn)V波器。而卡爾曼濾波是用前一個(gè)估計(jì)值和最近一個(gè)觀察數(shù)據(jù)來估計(jì)信號(hào)的當(dāng)前值，是用狀態(tài)方程和遞推的方法進(jìn)行估計(jì)的，其解是以估計(jì)值形式給出。因此稱這種系統(tǒng)為線性最優(yōu)估計(jì)器或?yàn)V波器?？柭^濾中信號(hào)和噪聲是狀態(tài)方程和量測(cè)方程表示的，因此設(shè)計(jì)卡爾曼濾波器要求已知狀態(tài)方程和量測(cè)方程。標(biāo)準(zhǔn)卡爾曼濾波器是在最小均方誤差準(zhǔn)則下的最佳線性過濾器，就是說，它使系統(tǒng)的狀態(tài)向量和狀態(tài)向量的預(yù)測(cè)值之間的均方誤差達(dá)到最小，它用

19、狀態(tài)方程和遞推方法進(jìn)行估計(jì)，它的解是以估計(jì)值形式給出的。由于它能夠?qū)ξ矬w的運(yùn)動(dòng)建立某種模型，因此在跟蹤中經(jīng)常被用到。當(dāng)觀測(cè)方程不是線性時(shí)，上述標(biāo)準(zhǔn)卡爾曼濾波方程不再適用，但是如果狀態(tài)估計(jì)值離真實(shí)值不是很遠(yuǎn)，可以將觀測(cè)方程局部線性化，得到擴(kuò)展卡爾曼濾波器(extended Kalman filtering，EKF)。由于EKF使用泰勒展開的一階近似，跟蹤一段時(shí)間之后，經(jīng)常會(huì)引起很大的參數(shù)估計(jì)的累計(jì)誤差。為此，無跡卡爾曼濾波（Unscented Kalman Filter ，UKF) 不再近似估計(jì)觀測(cè)方程，它仍然用高斯隨機(jī)變量表示狀態(tài)分布，不過是用特定選擇的樣本點(diǎn)加以描述。與EKF 相比，UKF

20、的誤差僅僅出現(xiàn)在三階以上的矩中，而且計(jì)算也簡(jiǎn)單，而EKF 僅僅精確到一階矩?？偟膩碚f，卡爾曼濾波是一個(gè)線性的估計(jì)器，能夠有效地跟蹤物體的運(yùn)動(dòng)和形狀變化，但它基于兩個(gè)假設(shè)：一是背景相對(duì)干凈；二是運(yùn)動(dòng)參數(shù)服從高斯分布9。因而適用范圍有限，對(duì)于復(fù)雜的多峰情況，還得求助于其它方法。2.2 卡爾曼濾波器的原理卡爾曼濾波的含義是現(xiàn)時(shí)刻的最佳估計(jì)為在前一時(shí)刻的最佳估計(jì)的基礎(chǔ)上根據(jù)現(xiàn)時(shí)刻的觀測(cè)值作線性修正?？柭鼮V波在數(shù)學(xué)上是一種線性最小方差統(tǒng)計(jì)估算方法，它是通過處理一系列帶有誤差的實(shí)際測(cè)量數(shù)據(jù)而得到物理參數(shù)的最佳估算。其實(shí)質(zhì)要解決的問題是要尋找在最小均方誤差下的估計(jì)值。它的特點(diǎn)是可以用遞推的方法計(jì)算，其所需

21、數(shù)據(jù)存儲(chǔ)量較小，便于進(jìn)行實(shí)時(shí)處理。具體來說，卡爾曼濾波就是要用預(yù)測(cè)方程和測(cè)量方程對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)。設(shè)動(dòng)態(tài)系統(tǒng)的狀態(tài)方程和測(cè)量方程分別為： (2-1) (2-2)上兩式子中，是k時(shí)刻的系統(tǒng)狀態(tài)，和是k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，是k時(shí)刻的測(cè)量值，是測(cè)量系統(tǒng)的參數(shù)，和分別表示過程和測(cè)量的噪聲，他們被假設(shè)成高斯白噪聲。如果被估計(jì)狀態(tài)和觀測(cè)量是滿足上述第一式，系統(tǒng)過程噪聲和觀測(cè)噪聲滿足第二式的假設(shè)，k時(shí)刻的觀測(cè)的估計(jì)可按下述方程求解。進(jìn)一步預(yù)測(cè)： (2-3)狀態(tài)估計(jì)： (2-4) 濾波增益矩陣： (2-5) 一步預(yù)測(cè)誤差方差陣： (2-6) 估計(jì)誤差方差陣： (2-7)上述就是卡爾曼濾波器的5條

22、基本公式，只有給定初值和，根據(jù)k時(shí)刻的觀測(cè)值，就可以遞推計(jì)算得k時(shí)刻的狀態(tài)估計(jì)（K=1，2，N）。2.2.1 擴(kuò)張卡爾曼濾波由于在實(shí)際中廣泛存在的是非線性狀態(tài)空間模型，使得常規(guī)卡爾曼濾波在電能質(zhì)量分析中的應(yīng)用存在困難，于是便出現(xiàn)了諸多針對(duì)非線性模型的次優(yōu)方10，其中應(yīng)用最廣泛的是擴(kuò)展卡爾曼濾波(extended Kalman filtering，EKF)。EKF 是將非線性系統(tǒng)線性化，與線性卡爾曼濾波公式完全類似。其主要思想是對(duì)非線性函數(shù)的泰勒展開式進(jìn)行截?cái)?，?shí)現(xiàn)非線性函數(shù)的線性化。根據(jù)泰勒展開式進(jìn)行的是1 階還是2 階截取，EKF 主要分為1 階EKF(first order EKF)和2

23、階EKF(second order EKF)。電能質(zhì)量分析中最常用的是1 階EKF，原理簡(jiǎn)述如下。假如非線性系統(tǒng)可表示為x(t)= f x(t),t+w(t) (2-8) y(t)=hx(t),t+v(t) (2-9)式中：x(t)為系統(tǒng)狀態(tài)向量；y(t)為系統(tǒng)量測(cè)向量；f 和h 是關(guān)于狀態(tài)的非線性函數(shù)；w 和v 均是均值為零的高斯白噪聲。式(2-8)(2-9)分別是狀態(tài)方程和量測(cè)方程。為了使卡爾曼濾波應(yīng)用到非線性系統(tǒng)中，非線性系統(tǒng)必須在指定位置進(jìn)行泰勒展開，實(shí)現(xiàn)線性化。推導(dǎo)過程如下：利用泰勒公式，分別在和 處對(duì)狀態(tài)方程和觀測(cè)方程進(jìn)行 1階泰勒展開，可得 （2-10） (2-11) 假設(shè) （2

24、-12） (2-13) (2-14) 則式(2-10)(2-11)可改寫成與常規(guī)卡爾曼方程相似的形式： (2-15) (2-16) 1階EKF遞推方程組與常規(guī)卡爾曼濾波遞推方程組在形式上相同，不同的是:KF中的和被1階EKF中的Jacobian 矩陣At和Ht代替，并且預(yù)測(cè)平均值和預(yù)測(cè)的冗余在EKF 中也分別計(jì)算,其遞推方程與卡爾曼濾波相同。在電能質(zhì)量分析中，A、B 矩陣的設(shè)計(jì)略有不同。1991 年，Beides H M 和Heydt G T 提出用擴(kuò)展卡爾曼濾波獲得電力系統(tǒng)諧波的動(dòng)態(tài)狀態(tài)估計(jì)，經(jīng)過實(shí)驗(yàn)室仿真和實(shí)測(cè)試驗(yàn)證明擴(kuò)展卡爾曼濾波能動(dòng)態(tài)地追蹤諧波內(nèi)容和時(shí)間。1993 年，Kamwa也將E

25、KF 引入電力系統(tǒng)電能質(zhì)量分析中，用于測(cè)量閃變。雖然擴(kuò)展卡爾曼濾波有很好的發(fā)展前景，但它在實(shí)際應(yīng)用中存在明顯的缺陷：一是線性化有可能產(chǎn)生極不穩(wěn)定的濾波；二是EKF需要計(jì)算Jacobian 矩陣的導(dǎo)數(shù)，實(shí)現(xiàn)起來較為復(fù)雜，而對(duì)于一些不可微的情況,EKF可能失效。在模型非線性較強(qiáng)以及系統(tǒng)噪聲非高斯時(shí)，估計(jì)的精度嚴(yán)重降低，甚至?xí)斐蔀V波器發(fā)散。2.2.2 無跡卡爾曼濾波為了更精確地?cái)M合非線性函數(shù)，Julier 提出了無跡變換(unscented transformation，UT)和無跡卡爾曼濾波(unscented Kalman filtering，UKF)。通過結(jié)合無跡變換和無跡卡爾曼濾波來實(shí)現(xiàn)非

26、線性系統(tǒng)的狀態(tài)估計(jì)，是近年來用于解決該問題的一種新的熱點(diǎn)方法。它通過一組精確選擇的Sigma 點(diǎn)來匹配隨機(jī)量的統(tǒng)計(jì)特性，UKF 沒有涉及非線性映射函數(shù)的Jacobian 矩陣計(jì)算問題，從而使算法的實(shí)現(xiàn)比EKF 更為容易, 在保持相當(dāng)運(yùn)算量的同時(shí)，具有更高的估計(jì)精度和更廣泛的適用范圍。傳統(tǒng)的線性化方法是對(duì)非線性映射本身做某種線性近似，然后再應(yīng)用線性估計(jì)的各種方法。而Julier S. J.等人提出的無跡變換則是基于用有限的參數(shù)來近似隨機(jī)量的概率統(tǒng)計(jì)特性要比近似任意的非線性映射函數(shù)更為容易的思想11。無跡變換的基本步驟可概括為：關(guān)于x 的Sigma 點(diǎn)集的產(chǎn)生不確定性的非線性變換與傳遞關(guān)于y 的統(tǒng)

27、計(jì)特性的推算。無跡變換Sigma 點(diǎn)集的選取方式不同，會(huì)產(chǎn)生很多種變換的演變形式，其目的主要是進(jìn)一步提高變換的精度，增強(qiáng)算法的穩(wěn)定性和減小運(yùn)算量等。在應(yīng)用UKF 時(shí)首先要對(duì)狀態(tài)量進(jìn)行擴(kuò)展，也就是將模型噪聲也作為狀態(tài)量的一部分，相應(yīng)地，無跡變換中用到的Sigma 點(diǎn)也需要擴(kuò)展，具體表示如下。擴(kuò)展?fàn)顟B(tài)方程的初始值： （2-17） （2-18）式中：為模型初始狀態(tài)變量；和 分別為擴(kuò)展?fàn)顟B(tài)向量的均值和協(xié)方差陣。Sigma 點(diǎn)集的創(chuàng)建通過下式實(shí)現(xiàn)： (2-19) 式中i=0,n,n=1,2m,m為預(yù)測(cè)空間維數(shù)；表示矩陣 平方根的第i個(gè)行向量或列向量，而矩陣平方根的常見求法是采用Cholesky分解；為k

28、1點(diǎn)處的協(xié)方差； ，決定 Sigma 取的點(diǎn)數(shù)，是由和參數(shù)決定的函數(shù)，為控制周圍的高階非線性值的參數(shù)，是介于0.000 1 1 之間的一個(gè)常數(shù)，是次要的比例參數(shù)，通常設(shè)置為 0 或3n ，以確保 Sigma點(diǎn)分布的峭度與高斯分布的峭度一致。Sigma 點(diǎn)向量通過狀態(tài)方程的非線性影射得到： (2-20)擴(kuò)展?fàn)顟B(tài)量的1 步預(yù)測(cè)為 (2-21) 擴(kuò)展?fàn)顟B(tài)量1 步預(yù)測(cè)的協(xié)方差陣為 (2-22)然后計(jì)算量測(cè)空間。量測(cè)空間的Sigma點(diǎn)集 的創(chuàng)建通過下式實(shí)現(xiàn)：1 步預(yù)測(cè)的擴(kuò)展?fàn)顟B(tài)Sigma 點(diǎn)向量經(jīng)過觀測(cè)方程的非線性映射得到： (2-23)觀測(cè)量的1 步預(yù)測(cè)為 (2-24)式中： ； 為合并高階狀態(tài)分布的

29、先驗(yàn)知識(shí)，高斯分布的最佳選擇是2。觀測(cè)量1 步預(yù)測(cè)的協(xié)方差陣為 （2-25）計(jì)算濾波增益矩陣： （2-26） 更新估計(jì)： （2-27） UKF在保持相當(dāng)運(yùn)算量的同時(shí)具有更高的估計(jì)精度和更為廣泛的適用范圍，它在國(guó)內(nèi)的相關(guān)研究起步較晚，但發(fā)展很快?？刹榈墓_資料主要集中于最近的23 年內(nèi)，而且在電力系統(tǒng)尤其是電能質(zhì)量方面的研究成果比較少。2.3 卡爾曼濾波器的應(yīng)用卡爾曼濾波器（Kalman Filter）是一個(gè)最優(yōu)化自回歸數(shù)據(jù)處理算法（optimal recursive data processing algorithm），它的廣泛應(yīng)用已經(jīng)超過30年，包括航空器軌道修正、機(jī)器人系統(tǒng)控制、雷達(dá)系統(tǒng)與

30、導(dǎo)彈追蹤等。近年來更被應(yīng)用于組合導(dǎo)航與動(dòng)態(tài)定位，傳感器數(shù)據(jù)融合、微觀經(jīng)濟(jì)學(xué)等應(yīng)用研究領(lǐng)域。特別是在圖像處理領(lǐng)域如頭臉識(shí)別、圖像分割、圖像邊緣檢測(cè)等當(dāng)前熱門研究領(lǐng)域占有重要地位。卡爾曼濾波作為一種數(shù)值估計(jì)優(yōu)化方法，與應(yīng)用領(lǐng)域的背景結(jié)合性很強(qiáng)。因此在應(yīng)用卡爾曼濾波解決實(shí)際問題時(shí)，重要的不僅僅是算法的實(shí)現(xiàn)與優(yōu)化問題，更重要的是利用獲取的領(lǐng)域知識(shí)對(duì)被認(rèn)識(shí)系統(tǒng)進(jìn)行形式化描述，建立起精確的數(shù)學(xué)模型，再?gòu)倪@個(gè)模型出發(fā)，進(jìn)行濾波器的設(shè)計(jì)與實(shí)現(xiàn)工作。濾波器實(shí)際實(shí)現(xiàn)時(shí)，測(cè)量噪聲協(xié)方差R一般可以觀測(cè)得到，是濾波器的已知條件。它可以通過離線獲取一些系統(tǒng)觀測(cè)值計(jì)算出來。通常，難確定的是過程激勵(lì)噪聲協(xié)方差的Q值，因?yàn)槲覀?/p>

31、無法直接觀測(cè)到過程信號(hào)。一種方法是通過設(shè)定一個(gè)合適的Q，給過程信號(hào)“注入”足夠的不確定性來建立一個(gè)簡(jiǎn)單的可以產(chǎn)生可接受結(jié)果的過程模型。為了提高濾波器的性能，通常要按一定標(biāo)準(zhǔn)進(jìn)行系數(shù)的選擇與調(diào)整?；究柭鼮V波（KF）器限定在線性的條件下，在大多數(shù)的非線性情形下，使用擴(kuò)展的卡爾曼濾波（EKF）器來對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)。隨著卡爾曼濾波理論的發(fā)展，一些實(shí)用卡爾曼濾波技術(shù)被提出來，如自適應(yīng)濾波，次優(yōu)濾波以及濾波發(fā)散抑制技術(shù)等逐漸得到廣泛應(yīng)用。其它的濾波理論也迅速發(fā)展，如線性離散系統(tǒng)的分解濾波（信息平方根濾波，序列平方根濾波，UD 分解濾波），魯棒濾波（H 波）。第三章 時(shí)變隨機(jī)信號(hào)及其測(cè)量過程數(shù)學(xué)模型的

32、建立3.1 隨機(jī)信號(hào)簡(jiǎn)介隨機(jī)信號(hào)又稱為不確定信號(hào)，是指無法用確定的時(shí)間函數(shù)來表達(dá)的信號(hào)。一般這類信號(hào)的頻域是連續(xù)的，而函數(shù)信號(hào)為斷續(xù)的。隨機(jī)信號(hào)是不能用確定的數(shù)學(xué)關(guān)系式來描述的，不能預(yù)測(cè)其未來任何瞬時(shí)值，任何一次觀測(cè)只代表其在變動(dòng)范圍中可能產(chǎn)生的結(jié)果之一，其值的變動(dòng)服從統(tǒng)計(jì)規(guī)律。它不是時(shí)間的確定函數(shù)，其在定義域內(nèi)的任意時(shí)刻沒有確定的函數(shù)值12。3.2 一維時(shí)變隨機(jī)信號(hào)的數(shù)學(xué)模型對(duì)每一確定的取樣時(shí)刻k，x（k）是一個(gè)隨機(jī)變量。當(dāng)取樣時(shí)刻的時(shí)標(biāo)k變化時(shí)，我們就得到一個(gè)離散的隨機(jī)過程，即隨機(jī)系列x（k）。假設(shè)待估隨機(jī)信號(hào)的數(shù)學(xué)模型是一個(gè)由白噪聲系列w（k）驅(qū)動(dòng)的一階自遞歸過程，其動(dòng)態(tài)方程為： （3-

33、1）式中：參數(shù)（3-1）式中的稱為過程噪聲或動(dòng)態(tài)噪聲。當(dāng)時(shí)標(biāo)k變化時(shí)，它將構(gòu)成一個(gè)白噪聲序列，其統(tǒng)計(jì)特性可用以下數(shù)字特診來描述：均值： （3-2）方差： 常數(shù) （3-3）自相關(guān)序列： （3-4）由式（3-1）式所決定的信號(hào)x（k），當(dāng)時(shí)標(biāo)k變化時(shí)，將構(gòu)成一個(gè)平穩(wěn)隨機(jī)x（k），其統(tǒng)計(jì)特性可用以下數(shù)字特性來描述：1.均值： （3-5）2.方差： 常數(shù) （3-6）3.自相關(guān)序列 當(dāng)取樣時(shí)刻的時(shí)標(biāo)k變化時(shí)，取樣時(shí)刻時(shí)標(biāo)相差j的x（k）的兩樣值間的自相關(guān)序列為 （3-7）3.3 信號(hào)測(cè)量過程的數(shù)學(xué)模型信號(hào)測(cè)量過程的數(shù)學(xué)模型，可用如下的測(cè)量方差給出 （3-8）式中：為k時(shí)刻的信號(hào)值。為該時(shí)刻對(duì)進(jìn)行測(cè)量所得

34、到的信號(hào)測(cè)量樣值。為此時(shí)在測(cè)量過程中所引入的量測(cè)噪聲，可將其視為獨(dú)立的附加白噪聲。當(dāng)k變化時(shí)，將組成一個(gè)隨機(jī)信號(hào)序列，將組成一個(gè)測(cè)量樣值序列，而將組成一個(gè)附加白噪聲序列。C為量測(cè)參數(shù)，它是一個(gè)由測(cè)量系統(tǒng)和測(cè)量方法所確定的不隨時(shí)間變化的常數(shù)。因?yàn)榱繙y(cè)噪聲序列是一個(gè)白噪聲序列，故其統(tǒng)計(jì)特性可用如下的數(shù)字特征描述：均值： （3-9）方差： 常數(shù) （3-10）自相關(guān)序列： （3-11） 又因量測(cè)噪聲序列與隨機(jī)信號(hào)序列互不相關(guān)，故 所以，我們可以看到一維時(shí)變隨機(jī)信號(hào)及其測(cè)量過程的數(shù)學(xué)模型，見圖3-1.圖3-1 一維時(shí)變信號(hào)及其測(cè)量過程的數(shù)學(xué)模型第四章 向量卡爾曼濾波和預(yù)測(cè)的一般方法4.1 標(biāo)量卡爾曼濾波

35、器的基本內(nèi)容一維隨機(jī)信號(hào)的遞歸型估計(jì)器的一般表達(dá)式： （4-1）在信號(hào)數(shù)學(xué)模型為（3-1）式、測(cè)量過程的數(shù)學(xué)模型為（3-2）的條件下，以均方根估計(jì)誤差最小為準(zhǔn)則對(duì)估計(jì)器的加權(quán)系數(shù)和進(jìn)行最優(yōu)化，并推導(dǎo)出標(biāo)量卡爾曼濾波器的最有估計(jì)的遞推算法。由（3-1）式表達(dá)的遞歸型估計(jì)器在k時(shí)刻對(duì)信號(hào)的估計(jì)誤差為 （4-2） 均方估計(jì)差為： （4-3） 若將（3-1）式代入（4-3）式，可得 （4-4） 若令對(duì)呵的偏導(dǎo)數(shù)為零，即 （4-5） （4-6）則由（4-5）式和（4-6）式中解出和將保證該遞歸型估計(jì)器的均方估計(jì)誤差為最小。根據(jù)統(tǒng)計(jì)理論中的正交原理，我們也可將（4-5）式和（4-6）式分別寫成正交方程的形

36、式，即 （4-7） （4-8）由（4-5）式，我們可得 （4-9）再由（4-9）式出發(fā)，經(jīng)過一系列的代換可求出 （4-10）此式為經(jīng)過最優(yōu)化得到的表達(dá)式。式中：是最優(yōu)遞歸型估計(jì)器的一個(gè)時(shí)變?cè)鲆?，它將隨時(shí)標(biāo)k的改變而變化。a是信號(hào)模型中反映一階自遞歸過程惰性大小的參數(shù)，只要信號(hào)模型確定后，它就是一個(gè)常數(shù)。顯然，和a是兩個(gè)意義完全不同的量。我們還可以看出，由于式中還包含另一個(gè)未知的時(shí)變?cè)鲆?，因此它?shí)際上只是一個(gè)和的關(guān)系式。要想最終確定，還必須求出。最優(yōu)遞歸型估計(jì)器對(duì)信號(hào)的均方估計(jì)誤差可寫成 （4-11）由正交公式（4-7）式和（4-8）式可知，上式等號(hào)右側(cè)的后兩項(xiàng)為零，故 （4-12） 由測(cè)量方程

37、（3-2）式，我們可得 （4-13）代入（4-12）式，因?yàn)樾盘?hào)與測(cè)量噪聲不相關(guān)，（k-1）時(shí)刻的信號(hào)估計(jì)值與k時(shí)刻的量測(cè)噪聲也補(bǔ)相關(guān)，故 （4-14） 我們還可以把最優(yōu)遞歸型估計(jì)器對(duì)信號(hào)的均方估計(jì)誤差寫成 （4-15）再利用和的關(guān)系式（4-10）式 （4-16）因?yàn)?、和互不相關(guān)，它們的交叉乘積項(xiàng)的均值都為零，故 （4-17）將（4-13）式代入（4-15）式，經(jīng)整理后求解得 （4-18）此式即經(jīng)過最優(yōu)化所得到的的表達(dá)式。4.2 最優(yōu)遞歸型估計(jì)器的構(gòu)成 由（3-1）式 （4-19） 所表述的遞歸型估計(jì)器，當(dāng)其時(shí)變?cè)鲆婧徒?jīng)過最優(yōu)化，即分別有（4-11）式和（4-16）式給出時(shí)，就是一個(gè)最優(yōu)遞歸型

38、估計(jì)器，其均方估計(jì)誤差最小。利用（4-11）式，我們可從（3-1）式中消去，得到 （4-20）由（4-20）式，可構(gòu)成一個(gè)最優(yōu)遞歸型估計(jì)器-標(biāo)量科爾曼濾波器，其框圖如圖4-1.圖4-1 標(biāo)量卡爾曼濾波器框圖對(duì)（4-20）式物理意義的說明：在尚未獲得k時(shí)刻的新測(cè)量樣值以前，我們只能從（k-1）時(shí)刻對(duì)信號(hào)所作出的估計(jì)出發(fā)，根據(jù)由信號(hào)數(shù)學(xué)模型所確定的規(guī)律來對(duì)k時(shí)刻的信號(hào)進(jìn)行預(yù)測(cè)。由于信號(hào)數(shù)學(xué)模型中的動(dòng)態(tài)噪聲的確切數(shù)值無從得知，故對(duì)的預(yù)估值只能取作?？梢?，（4-20）式等號(hào)右側(cè)的第一項(xiàng)就是在未獲得任何新信息的情況下，根據(jù)以往的測(cè)量數(shù)據(jù)對(duì)k時(shí)刻的信號(hào)所做的預(yù)估。在k 時(shí)刻的新測(cè)量樣值尚未得到之前，我們還

39、可對(duì)k時(shí)刻的將要測(cè)得的新測(cè)量樣值進(jìn)行預(yù)估。但是，此時(shí)我們只能從對(duì)k時(shí)刻的信號(hào)的預(yù)估值出發(fā)，根據(jù)量測(cè)方程來對(duì)k時(shí)刻將要測(cè)得的作出預(yù)估。由于量測(cè)噪聲的確切數(shù)值無從得知，故對(duì)的預(yù)估值只能取作 （4-21）當(dāng)我們測(cè)得k時(shí)刻的新測(cè)量樣值后，若所測(cè)得的值與其預(yù)估值 之差不為零，就說明k時(shí)刻的新測(cè)量樣值中包含有前（k-1）次測(cè)量中所沒有的新信息。若與其預(yù)估值之差為零，則說明k時(shí)刻的新測(cè)量樣值中不包含任何新信息。因此，我們把k時(shí)刻的信號(hào)實(shí)測(cè)值與其預(yù)估值之差稱為第k 次測(cè)量中的新信息。顯然，當(dāng)我們測(cè)得k時(shí)刻的新測(cè)量樣值之后，可利用第k次測(cè)量中的新信息乘上一個(gè)比例系數(shù)作為修正項(xiàng)，對(duì)未測(cè)得前對(duì)信號(hào)給出的預(yù)估值進(jìn)行修

40、正，從而得到k時(shí)刻對(duì)信號(hào)的估計(jì)值?？梢?，（4-20）式等號(hào)右側(cè)的第二項(xiàng)即為對(duì)信號(hào)預(yù)估值的修正項(xiàng)。4.3 標(biāo)量卡爾曼濾波器的遞推算法卡爾曼濾波的基本算法是預(yù)估加修正，而公式（4-20）式、（4-21）式和（4-13）式就構(gòu)成了標(biāo)量卡爾曼濾波器在信號(hào)及其測(cè)量過程的數(shù)學(xué)模型分別為和)時(shí)對(duì)信號(hào)進(jìn)行最優(yōu)估計(jì)的一套完整的遞推算法。（4-6）式可用來推算卡爾曼濾波器在不同取樣時(shí)刻k對(duì)信號(hào)的估計(jì)值，即 （4-22）（4-20）式可用來推算卡爾曼濾波器在不同取樣時(shí)刻k的時(shí)變?yōu)V波增益，即 （4-23）（4-13）式可用來推算卡爾曼濾波器在不同取樣時(shí)刻k的均方估計(jì)誤差，即 （4-24） 為了便于將標(biāo)量卡爾曼濾波器的

41、遞推算法直接推廣到向量隨機(jī)信號(hào)（即多維隨機(jī)信號(hào)）的卡爾曼濾波中去，給出如下的一套完整的遞推算法：濾波估計(jì)方程 （4-25）濾波增益方程 （4-26）式中：均方誤差濾波方程： （4-27）4.4 標(biāo)量卡爾曼預(yù)測(cè)器假設(shè)待測(cè)量隨機(jī)信號(hào)的數(shù)學(xué)模型為一階自遞歸過程，即 （4-28）信號(hào)測(cè)量過程的數(shù)學(xué)模型為 （4-29）線性遞歸型預(yù)測(cè)估計(jì)器可用如下的遞推方程來表述： （4-30）為使（4-30）式表述的線性預(yù)估其成為一個(gè)最優(yōu)線性預(yù)估器，就必須依據(jù)最小均方誤差準(zhǔn)則對(duì)（4-30）式中的時(shí)變預(yù)測(cè)增益和進(jìn)行最優(yōu)化。用上節(jié)類似的方法，可以求得經(jīng)最優(yōu)化后的和的表達(dá)式以及相應(yīng)的均方預(yù)測(cè)誤差的表達(dá)式，即 （4-31） （

42、4-32） （4-33）其中：根據(jù)（4-31）式，可由推算出；再根據(jù)（4-32）式又可由推算出。只要選定了初值，這一遞歸過程就可以不斷的進(jìn)行下去。將（4-31）式代入（4-30）式，可得 （4-34） 根據(jù)（4-34）式，可以構(gòu)成一個(gè)標(biāo)量卡爾曼預(yù)測(cè)器，其框圖如圖4-2所示。圖4-2 標(biāo)量卡爾曼預(yù)測(cè)期框圖圖3-2所示的標(biāo)量卡爾曼預(yù)測(cè)器的一套完整的遞推算法有（4-31）式、（4-32）式、（4-33）式組成。但為了便于將這組公式直接推廣到向量隨機(jī)信號(hào)（即多維隨機(jī)信號(hào)）的卡爾曼遞推預(yù)測(cè)中去，當(dāng)變量隨機(jī)信號(hào)及其測(cè)量過程的數(shù)學(xué)模型分別為和時(shí)，將（4-33）式作下改寫，即利用（4-32）式消去（4-33）

43、式中的。經(jīng)過改寫后得到卡爾曼預(yù)測(cè)器對(duì)此信號(hào)進(jìn)行遞推預(yù)測(cè)的一套完整的遞推算法，公式如下：預(yù)測(cè)估計(jì)方程 （4-35） 預(yù)測(cè)增益方程 （4-36）均方預(yù)測(cè)誤差方程 （4-37）4.5 向量卡爾曼濾波和預(yù)測(cè)與標(biāo)量情況類似，可用向量表示k時(shí)刻對(duì)隨機(jī)信號(hào)向量的最優(yōu)線性濾波估計(jì)值，用向量表示在k時(shí)刻對(duì)（k+1）時(shí)刻的隨機(jī)信號(hào)向量的最優(yōu)線性預(yù)測(cè)估計(jì)值。 對(duì)向量卡爾曼濾波來說，標(biāo)量情況下的濾波誤差此時(shí)將變成一個(gè)濾波誤差向量 （4-38）標(biāo)量情況下的均方濾波誤差，此時(shí)將變成一個(gè)濾波誤差的協(xié)方差矩陣 （4-39）同樣，對(duì)向量卡爾曼預(yù)測(cè)來說，其預(yù)測(cè)誤差向量為 （4-40）其預(yù)測(cè)協(xié)方差矩陣為 （4-41）所謂向量卡爾曼

44、濾波器或預(yù)測(cè)器，實(shí)際上就是一種能對(duì)向量隨機(jī)信號(hào)進(jìn)行最優(yōu)線性濾波或預(yù)測(cè)的遞歸型濾波器?！白顑?yōu)”的含義，是指能使每個(gè)信號(hào)分量的均方估計(jì)誤差同時(shí)為最小。 考慮到向量卡爾曼濾波器或預(yù)測(cè)與標(biāo)量卡爾曼濾波器或預(yù)測(cè)無論條件上（即信號(hào)及其測(cè)量過程的數(shù)學(xué)模型）還是在要求上（即“最優(yōu)”的含義）都完全相似，因而，可以直接把標(biāo)量卡爾曼濾波或預(yù)測(cè)的遞推公式推廣到向量情況。 在直接引用標(biāo)量卡爾曼濾波或預(yù)測(cè)的遞推公式時(shí)，除了應(yīng)把標(biāo)量形式的信號(hào)、信號(hào)估計(jì)值和測(cè)量樣值表述成向量形式，把系統(tǒng)參數(shù)a和量測(cè)參數(shù)c表述成系統(tǒng)矩陣（或稱系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣）A和量測(cè)矩陣C，把均方估計(jì)誤差和有關(guān)噪聲和方差表述成相應(yīng)的協(xié)方差矩陣外，還應(yīng)根據(jù)表

45、3-1將遞推公式中的標(biāo)量運(yùn)算變換成相應(yīng)的矩陣運(yùn)算。表4-1 從標(biāo)量運(yùn)算到矩陣運(yùn)算的轉(zhuǎn)換表注意：在使用表4-1時(shí)必須使變換后的沒一步矩陣運(yùn)算都能進(jìn)行下去，并使等號(hào)兩側(cè)最終得到的矩陣在階數(shù)上保持一致。我們可以從標(biāo)量卡爾曼濾波的遞推算法（4-26）（4-29）式直接得到向量卡爾曼濾波的遞推算法：濾波估計(jì)方程： （4-42）濾波增益方程： （4-43）式中： 濾波協(xié)方差方程： （4-44） 和標(biāo)量卡爾曼濾波器一樣，向量卡爾曼濾波器也是以預(yù)測(cè)加修正作為其遞推濾波的基本算法的?？柭鼮V波器的這一特性，使得很容易用計(jì)算機(jī)來實(shí)現(xiàn)對(duì)信號(hào)的實(shí)時(shí)濾波，為此，可采用軟件方案來實(shí)現(xiàn)卡爾曼濾波。 在向量卡爾曼濾波的一整套

46、遞推算法中，（4-42）式可構(gòu)成向量卡爾曼濾波的主程序算法， 圖4-3給出了主程序算法的框圖。圖 4-3 向量卡爾曼濾波的主程序算法框圖從圖4-3中可以看出，向量卡爾曼濾波的主程序算法主要分三步來進(jìn)行。 第一步：在已知（k-1）時(shí)刻對(duì)信號(hào)向量的估計(jì)值的條件下，用系統(tǒng)矩陣A乘以，得到在（k-1）時(shí)刻對(duì)k時(shí)刻信號(hào)向量的預(yù)測(cè)值。 第二步：用量測(cè)矩陣C乘以，得到在（k-1）時(shí)刻對(duì)k時(shí)刻的測(cè)量數(shù)據(jù)向量的預(yù)測(cè)值；再用的實(shí)測(cè)值減去預(yù)測(cè)值，得到殘差（新信息）；最后用濾波增益矩陣乘以，得到修正量。 第三步：把對(duì)信號(hào)的預(yù)測(cè)值加上修正量，得到信號(hào)的濾波估計(jì)值。 值得注意的是：在上述運(yùn)算過程中，所用的濾波增益矩陣并不是在主程序中計(jì)算出來的，而是從向量卡爾曼濾波的子程序算法中計(jì)算出來的。上述運(yùn)算過程中所得到的值應(yīng)存儲(chǔ)起來，以供下一次遞推時(shí)使用。只要確定了信號(hào)估計(jì)值的初值，例如設(shè)，則隨著時(shí)間的推移，可在測(cè)得后算出，在測(cè)得后算出，依次類推。在從（k-1）時(shí)刻到k時(shí)刻這段時(shí)間內(nèi)，只需把存儲(chǔ)起來。隨著遞推的不斷進(jìn)行，再對(duì)它不斷更新。當(dāng)然，如果系統(tǒng)矩陣和量測(cè)矩陣是時(shí)變的，就需把和也存儲(chǔ)起來。 向量卡爾曼濾波的子程序算法是由（4-41）（4-44）式構(gòu)成的，其算法框圖由圖4-4所示。 從圖4-4中可以看出，向量卡爾曼濾波的子程序算法也分三步來進(jìn)行。 第一步：在以知和,