【正文】 將上式改寫成 ? ?? ?( 1 / 1 ) ( 1 / ) ( 1 ) ( 1 ) ( 1 / ) ( 1 )x k k x k k K k H k x k k v k? ? ? ? ? ? ? ? ? ? ? ? 則濾波估計誤差為 ? ?? ??( 1 / 1 ) ( 1 ) ( 1 / ) ( 1 ) ( 1 ) ( 1 / ) ( 1 )( 1 / ) ( 1 ) ( 1 ) ( 1 / ) ( 1 )x k k x k x k k K k H k x k k v kx k k K k H k x k k v k? ? ? ? ? ? ? ? ? ? ? ? ? ?? ? ? ? ? ? ? ? ? ? (5) 由于 ?( 1/ 1)x k k??是 ( 1)xk? 的最優(yōu)估值，估計誤差 ( 1/ 1)x k k??必須正交于( 1)zk? ，利用這個原理求解 ( 1)Kk? 。 (1) 假定由觀測值 (0), (1), , ( )z z z k? 估計得到狀態(tài)變量 ( 1)xk? 的一步最優(yōu)預(yù)測估值?( 1/ )xk k? 和觀測向量 ( 1)zk? 的預(yù)測估值 ?( 1/ )z k k? 為： ??( 1 / ) ( 1 ) ( 1 / )z k k H k x k k? ? ? ? ? (2) 當(dāng)獲得 ( 1)zk? 之后，求得 ( 1)zk? 與 ?( 1/ )z k k? 的誤差，即： ?( 1 / ) ( 1 ) ( 1 / ) ( 1 ) ( 1 / ) ( 1 )z k k z k z k k H k x k k v k? ? ? ? ? ? ? ? ? ? ? (3) 以 ( 1/ )z k k? 去修正 ?( 1/ )xk k? ，得到 ( 1)xk? 的濾波估值為： ? ?( 1 / 1 ) ( 1 / ) ( 1 ) ( 1 / )x k k x k k K k z k k? ? ? ? ? ? ? ? 或 ? ?? ? ?( 1 / 1 ) ( 1 / ) ( 1 ) ( 1 ) ( 1 ) ( 1 / )x k k x k k K k z k H k x k k? ? ? ? ? ? ? ? ? ? ? ? 式中， ( 1)Kk? 為待定的最優(yōu)增益矩陣。 離散系統(tǒng)卡爾曼最優(yōu)濾波基本方程的推導(dǎo) 先不考慮輸入控制信號的作用，則系統(tǒng)的狀態(tài)方程為： ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( )x t A t x t F t w tz t H t x t v t?? 式中維數(shù)同前所述。 狀態(tài)向量 ()xk 的初始統(tǒng)計特性是給定的，即： ? ?? ?? ?? ?00 0 0( 0)( 0) ( 0) TE x mE x m x m P?? ? ? 給出觀測序列 (0), (1), , ( )z z z k? ，要求找出 ()xj的線性最優(yōu)估計 ?( / )xj k ，使得估值江蘇科技大學(xué)本科畢業(yè)設(shè)計（論文） 21 ?( / )xj k 與 ()xj之間的誤差 ?( / ) ( ) ( / )x j k x j x j k??的方差和為最小，即： ? ? ? ?? ?? ?( ) ( / ) ( ) ( / ) m i nTE x j x j k x j x j k? ? ? 也就是要求各狀態(tài)變量估計誤差的方差和最小。 2122 0 4 5 ( ) , 5 4 2 8 ( ) , 6 0 ( / )7 7 8 5 0 , 7 6 5 1 0 , 1 . 4 8 8 , 1 . 7 1 2m k g I k g m v k m hk k a b? ? ? ?? ? ? ? 離散系統(tǒng)的卡爾曼濾波問題 設(shè)離散系統(tǒng)的差分方程和觀測方程為： ( 1 ) ( 1 , ) ( ) ( 1 , ) ( ) ( 1 , ) ( )( ) ( ) ( ) ( )x k k k x k G k k u k k k w kz k H k x k v k? ? ? ? ? ? ? ? ??? 式中 ()xk 是 n維狀態(tài)向量， ()uk 是 r維控制向量， ()zk 是 m維觀測向量， ( 1, )kk?? 是nn? 轉(zhuǎn)移矩陣， ( 1, )Gk k? 是 nr? 矩陣， ( 1, )kk?? 是 np? 矩陣， ()Hk是 mn? 矩陣。所謂最優(yōu)線性估計有一下三點含義： (1) 估值 1?( / )xt t 是 0( ) ( )z t t????， 的線性函數(shù)； (2) 估值是無偏的，即 ? ? ? ?11?( / ) ( )E x t t E x t? ； (3) 要求估值誤差 1 1 1?( / ) ( ) ( / )x t t x t x t t??的方差和為最小，即要求 ? ? ? ?? ?1 1 1 1? ?( ) ( / ) ( ) ( / ) = m i nTE x t x t t x t x t t? ? ? 根據(jù) t1和 t的 大小關(guān)系連續(xù)系統(tǒng)估計問題可分為三類： 江蘇科技大學(xué)本科畢業(yè)設(shè)計（論文） 20 第一類： 1tt? ,稱為 預(yù)測（或外推）問題； 第二類： 1tt? ，稱為濾波問題； 第三類： 1tt? ，稱為平滑（或內(nèi)插）問題。要求找出1()xt 的最優(yōu)線性估計 1?( / )xt t 。一般都采用線性最小方差估計。 ()xt 的初始狀態(tài) 0()xt 是一個隨機向量，假定 0()xt 的數(shù)學(xué)期望 ? ?00()E x t m? ,協(xié)方差矩陣 ? ? ? ?? ?0 0 0 0 0( ) ( ) ( ) TP t E x t m x t m? ? ? ?都為 已知。正定的物理意義是觀測向量 ()zt 的各分量都附加有隨機噪聲。若 ()wt 和 ()vt 相互獨立，它們的協(xié)方差矩陣分別為： ( ) ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) 0TTTE w t w Q t tE v t v R t tE w t v? ? ?? ? ???? ?????? ?????? ??? 式中的 ()t??? 是狄拉克（ Dirac） ? 函數(shù)，當(dāng) =t? 時， ( )=t????；當(dāng) t ?? 時， ( )=0t??? ，且 ( ) 1td? ? ???? ??? 當(dāng) ()Qt 和 ()Rt 不隨時間變化時 ， Q和 R都是白 噪聲的譜密度矩陣。要從夾雜有隨機噪聲的觀測信號中準(zhǔn)確的分離出狀態(tài)變量是不可能的，只有根據(jù)觀測信號來估計這些狀態(tài)變量。 對于實際控制系統(tǒng)，最優(yōu)控制律或自適應(yīng)控制律的形成需要系統(tǒng)的狀態(tài)變量，而狀態(tài)變量往往不能直接獲得，需要通過測量裝置進行觀測，根據(jù)觀測得到的信號來確定 狀態(tài)變量。 連續(xù)系統(tǒng)的卡爾曼濾波問題 在許多實際控制過程中，系統(tǒng)往往受到隨機干擾的作用。 式（ e）或（ e1）計算比較簡單，所以實際中總是使用這個公式，但是需要注意這個公式僅在使用最優(yōu)卡爾曼增益時它才成立。式（ e3）可保證協(xié)方差陣 P 的對稱性和正定性。 P(k | k) = P(k | k ?1) ? K(k)S(k)KT (k) （ e2） 其中，測量的預(yù)測協(xié)方差（或新息協(xié)方差） S(k) = C(k)P(k | k ?1)CT (k) + R(k)，用來衡量新息（測量值減去測量估計值）的不確定性，新息的協(xié)方差越小，說明測量值越精確。 卡爾曼濾波器的遞推算法如下： 狀態(tài)的一步預(yù)測方程（基于系統(tǒng)的上一個狀態(tài)） X? (k | k ?1) = A(k ?1)X (k ?1| k ?1) + B(k ?1)U(k ?1) （ a） 協(xié)方差的一步預(yù)測 P(k | k ?1) = A(k ?1)P(k ?1| k ?1)AT (k ?1) + Q(k ?1) (b) 濾波增益方程 K(k) = P(k | k ?1)CT (k)[C(k)P(k | k ?1)CT (k) + R(k)]?1 (c) 濾波估計方程 X? (k | k) = X? (k | k ?1) + K(k)[Z(k) ? C(k)X? (k | k ?1)] (d) 濾波協(xié)方差更新方程 江蘇科技大學(xué)本科畢業(yè)設(shè)計（論文） 18 P(k | k) = [I ? K(k)C(k)]P(k | k ?1) (e) 協(xié)方差的更新方程還有下面幾種不同的形式： P(k | k) = P(k | k ?1) ? K(k)C(k)P(k | k ?1) （ e1） 式（ e）和（ e1）相同，適用于增益為最優(yōu)卡爾曼增益的時候。系統(tǒng)的測量方程的輸出項 Z(k)是可以實際測量的量。時間更新過程根據(jù)當(dāng)前時刻的系統(tǒng)狀態(tài)獲得對下一時刻的先驗估計；量測更新過程將量測信息和先驗估計相結(jié)合獲得改進的系統(tǒng)的后驗估計。設(shè)計卡爾曼估計器需要系統(tǒng)的 狀態(tài)空間模型和過程與量測噪聲的方差值，它是對連續(xù)或離散問題的最優(yōu)解。 1960年卡爾曼 (kalman)提出了最優(yōu)線性遞推濾波算江蘇科技大學(xué)本科畢業(yè)設(shè)計（論文） 17 法，從而克服了維納濾波的缺點。 狀態(tài)估計的早期工作是由維 納 (Wiener)在 40年代末完成的，常稱為維納濾波器。復(fù)雜的工程系統(tǒng)都是為了實現(xiàn)對觀測數(shù)據(jù)的精確測量和對系統(tǒng)狀態(tài)的精確估計，但由于實際系統(tǒng)經(jīng)常受到各種隨機干擾的影響，系統(tǒng)觀測信息中夾雜有各種隨機噪聲，因此如何將系統(tǒng)中有用信號與隨機噪聲有效正確分離，以及從含有噪聲的觀測數(shù)據(jù)中準(zhǔn)確估計出所需要的系統(tǒng)信息等都是不可回避的實際問題。簡單的講，測量是通過儀器、傳感器等測量出系統(tǒng)中的某些數(shù)據(jù)，這些數(shù)據(jù)通常是還有測量噪聲的隨機變量或隨機過程；估計是從測量（或觀測）數(shù)據(jù)中濾去各種隨機噪聲，提取出所需的信息。 采用 狀 態(tài) 估計方法 預(yù)估汽車的一些不能直接測量的重要狀態(tài)量，如質(zhì)心側(cè)偏角、側(cè)傾角：或測量起來成本很高的狀態(tài)量，如橫擺角速度、側(cè)傾角速度。但實際上，無論是橫擺角速度還是質(zhì)心側(cè)偏角都很難用合理的成本直接測量，這就給控制算法的實用性提出了挑戰(zhàn)。另一方面，為了滿足汽車主動安全控制的需要，國內(nèi)外相繼開發(fā)了電子穩(wěn)定系統(tǒng) (ESP)、車輛動態(tài)控制系統(tǒng) (VDC)等，來實現(xiàn)緊急避障情況下的汽車輔助駕駛功能。 3. 在給出的整車參數(shù)下，進行模型仿真 ， 為后續(xù)的仿真計算奠定基礎(chǔ) 。 本章小節(jié) 1. 概述了整車動力學(xué)建模方法，結(jié)合 結(jié)合牛頓運動定律 、 汽車操縱動力學(xué)相關(guān)理論 ，建立了汽車二自由度的動力學(xué)模型、線性輪胎模型。,[2 0])。 plot(t,Y(:,1)) set(gca,39。0 1] D=[0。 m=3018 a= b= k1=23147 k2=38318 Jz=10437 V=120/ A=[(k1+k2)/(m*V),(a*k1b*k2)/(m*V)1 (a*k1b*k2)/Jz,(a^2*k1+b^2*k2)/(Jz*V)] B=[k1/(m*V),a*k1/Jz]39。 clc。 二自由度汽車運動微分方程式為： 江蘇科技大學(xué)本科畢業(yè)設(shè)計（論文） 13 ? ? ? ?? ? ? ?2221f r f r ffr ffrz z zK K a K b K Km u m umua K b K aKa K b KI I u I? ???? ???????????? ? ? ????????? ? ? ? ??? （ 213） 此模型集中了汽車的主要性能，把影響汽車性能的參數(shù)減至最少，可以得到數(shù)學(xué)模型的解析解，因而能得出普遍適用的結(jié)論。運用狀態(tài)空間的方法以狀態(tài)空間的形式表示汽車的動力學(xué)方程 ， 二自由度線性汽車模型狀態(tài)方程和量測方程為 ? ? ? ? ? ??????? ????????????????????AX （ 211） ? ? ? ? ? ?yaY C Drr? ?? ? ? ?? ? ?? ? ? ?? ? ? ? （ 212） 式中? ? ? ?? ? ? ?21 1 1 2 1 1222 1 2 2 2 11 1 1 2 1 122 1 2 2 2 11,0 1 0f r r f ftt tff r f rzzzf r r f ft t tk k a k b k km V m V mVa a bABa a b aka k b k a k b kJJ J Vk k b k a k kc c dCD m m V mc c d???? ??? ??? ??? ? ? ? ??? ? ? ?? ? ? ??? ????? ? ? ?????? ??????? ? ? ?? ? ? ?? ? ? ?? ? ? ?? ? ? ?? ? ? ?? ? ? ?? ? ? ?? ? ? ? 這種建模的方法要先在 MATLAB 環(huán)境下編寫 M 文件計算狀態(tài)空間的系數(shù)矩 陣 A、 B、 C和 D。這時最好通過添加具有一步積分延遲的 Memory模塊切斷代數(shù)環(huán) [3738]。對于常見的仿真所 用時間過長問題，可能有以下幾種原因：仿真步長和容許誤差過小，直接影響仿真所用時間；當(dāng)模型中含有 Matlab Fen模塊時，仿真每進行一步就要MATLAB解釋程序，會