+1時刻狀態(tài)估計協(xié)方差矩陣；Q為過程噪聲矩陣；R為測量噪聲矩陣；I為7X7維 單位矩陣；
[0088] 步驟4、根據(jù)步驟3設計的Kalman濾波方程對含有雙頻彈性干擾下的角速度進行 濾波處理。
【具體實施方式】 [0089] 二：
[0090] 本實施方式步驟1所述測量矩陣Η通過以下步驟得到：
[0091] 針對角速度的測量方程有如下形式
[0092] z = HX+w
[0093] 同時，根據(jù)角速度的測量方程z=x+Vi+vfw和X=[XViν2 ν3 ν4ωιω2]τ，得 到Η= [1 1 0 1 0 0 0]。
[0094] 其它步驟及參數(shù)與【具體實施方式】一相同。
【具體實施方式】 [0095] 三：
[0096] 本實施方式步驟1所述的將彈性干擾信號中的+約}進行處理得到Vl、V2， 將彈性干擾信號中的錢j進行處理得到v3、v4的過程如下：
[0097] 設巧％為vi關(guān)于時間的一階導數(shù)，則外=_9丨=?卿cos::(.釋+約.）； 對v2取時間的導數(shù)為A=-AWsin(w/十奶）=V
[0098]同理設％=:為'11{郵+爲），v4為v3關(guān)于時間的一階導數(shù)，則巧=4=&?2__(叫+?對 乂4取時間的導數(shù)為化(叫+妁）="和;；
[0099] 根據(jù)上述關(guān)系，將其整理成彈性干擾信號狀態(tài)方程的形式如下
[0101] 其它步驟及參數(shù)與【具體實施方式】二相同。
[0102] 實施例
[0103] 在本實施例中，角速度信號為X=asin(c〇t+<i))，其中a=0· 5，ω=1 (rad/s)， 相位Φ為隨機值在范圍[0, 2π)中均勻分布，選取φ= 0.7854 (rad);兩個低頻干擾信號 角頻率分別為Vi=b 和v2=b2sin(02t+<i)2)，其中 1^ =b2= 0·lXa，ω1 =20X2π(rad/s)，ω2= 50X2π(rad/s)，相位Φ!和Φ2為隨機值在范圍[0, 2π)中均 勻分布，選取t= 1.5057 (rad)，Φ2= 0.0025 (rad);角速度濾波器狀態(tài)初始值為X= [0 0 0 0 0 26 34]τ〇
[0104] 根據(jù)以上參數(shù)，按照【具體實施方式】三的步驟進行仿真實驗，實驗結(jié)果如圖1至圖6 所示。
[0105] 圖1顯示了角速度信號真值、測量值和估計值的比較結(jié)果?？梢钥吹綔y量值受到 了比較強烈的彈性振動信號和測量噪聲的干擾。本發(fā)明的濾波器比較顯著的濾除了振動 信號和測量噪聲的干擾，得到的測量信號的估計值和真值很接近而且信號沒有相位上的延 遲。圖2顯示了頻率為20Hz的彈性振動信號即彈性干擾狀態(tài)Vl下的估計結(jié)果。由于剛開始 濾波器設定的振動信號的頻率不準確（代表對振動信號頻率不具有準確信息），所以在前 0.2秒估計結(jié)果較差。但很快濾波器的估計結(jié)果就向真值收斂。圖3,圖4和圖5分別顯示 了彈性干擾狀態(tài)v2,vjPv4下的估計結(jié)果，類似于狀態(tài)vi的估計，在〇. 2秒后濾波器的估計 結(jié)果向真值收斂。圖6顯示了對兩個彈性干擾信號頻率的估計，對于頻率為20Hz的彈性信 號，濾波器初始值設置頻率為26Hz，對于頻率為50Hz的彈性信號，濾波器初始值設置頻率 為44Hz，代表對彈性干擾信號的頻率信息事先并不完全確定?？梢钥吹皆?. 2秒后，兩個頻 率的估計值就快速趨向真值并穩(wěn)定在真值上。在本實施例中，測量誤差標準差為〇. 〇42rad/ s，而估計誤差的標準差為0. 0081rad/s。估計誤差的標準差相對于測量誤差標準差減小了 81%〇
【主權(quán)項】
1. 一種雙頻彈性干擾下角速度濾波方法，其特征在于包括下述步驟： 步驟1、針對彈性干擾下角速度信號動態(tài)過程進行建模： 飛行器的角速度動態(tài)過程為C1) 其中，ω代表角速度，a代表角加速度，Μ為轉(zhuǎn)動力矩，J為轉(zhuǎn)動慣量； 彈性干擾信號為兩個正弦信號疊加而成，設兩個正弦信號的頻率分別為和ω2,單 位為rad/s巧個正弦信號的相位分別為I輯和依，幅度分別為b郝b2; 彈性干擾信號的形式如下<：2) 將彈性干擾信號中的Asini碑?+ 貨)進行處理得到彈性干擾狀態(tài)VI、V2,將彈性干擾信 號中的Λ:sin(啤+巧）進行處理得到彈性干擾狀態(tài)V3、V4; 將干擾頻率和ω2當做狀態(tài)進行估計；設狀態(tài)為X=[XViV2V3V4 ?1C〇2]T，其 中X為角速度信號，即X = ω ;則角速度和干擾信號的狀態(tài)方程為幻） 其中，a,角加速度的測量值； 角速度的測量方程為Z=X+V1+V3+W (4) 式中，Z為角速度的測量值;W是零均值高斯白噪聲； 測量矩陣為 Η= [1 1 0 1 0 0 0] (5) 步驟2、設計狀態(tài)轉(zhuǎn)移矩陣：(6) 式中其中，τ為測量周期； 步驟3、設計Kalman濾波方程： 角速度濾波器采用EKF設計，Kalman濾波器方程為式中，Λ+ι,α.，ι是對k+1時刻狀態(tài)的估計值；義是對k+1時刻狀態(tài)的預報值；Kk和KW 分別是k時刻和k+1時刻的濾波器增益；Zw為k+1時刻的測量值；Φw,k為k時刻到k+1 時刻的狀態(tài)轉(zhuǎn)移矩陣；為k+1時刻狀態(tài)預報的協(xié)方差矩陣；Pk,k和分別是k時刻 和k+1時刻狀態(tài)估計協(xié)方差矩陣；Q為過程噪聲矩陣；R為測量噪聲矩陣；I為7X7維單位 矩陣； 步驟4、根據(jù)步驟3設計的Kalman濾波方程對含有雙頻彈性干擾下的角速度進行濾波 處理。2. 根據(jù)權(quán)利要求1所述的一種雙頻彈性干擾下角速度濾波方法，其特征在于步驟1所 述測量矩陣Η通過W下步驟得到： 針對角速度的測量方程有如下形式Ζ = HX+W 同時，根據(jù)角速度的測量方程Z=X+Vi+V3+W和Χ=[XViV2V3V4 ?1C〇2]T，得到Η=[1 1 0 1 0 0 0]。3. 根據(jù)權(quán)利要求1或2所述的一種雙頻彈性干擾下角速度濾波方法，其特征在于步 驟1所述的將彈性干擾信號中的sin(巧/ +約）進行處理得到VI、V2,將彈性干擾信號中的 度2 5川(化/ +則進行處理得到乂3、乂4的過程如下： 設巧=6isin(嗎?+巧），:V2為V1關(guān)于時間的一階導數(shù)，則馬=? =南腳C附(蹲+巧）；對V2 取時間的導數(shù)為=-句份I2sin(邸+巧）=命'1; 同理設1? =Min(仿：/ + &)，vj為V3關(guān)于時間的一階導數(shù)，則+轉(zhuǎn));對V4 取時間的導數(shù)為，:'_4= -/，;雌:sin(似+巧）二啡S..';； 根據(jù)上述關(guān)系，將其整理成彈性干擾信號狀態(tài)方程的形式如下0,
【專利摘要】一種雙頻彈性干擾下角速度濾波方法，涉及一種角速度濾波方法，特別是涉及一種角速度擴展卡爾曼濾波方法。為了解決現(xiàn)有的傳遞函數(shù)形式濾波器在面臨不同頻率的彈性干擾時需要重新設計的問題和進行彈性干擾和測量噪聲濾除時產(chǎn)生相位延遲而導致的角速度信號估計值不準確的問題。本發(fā)明首先針對彈性干擾下角速度信號動態(tài)過程進行建模，并建立角速度和干擾信號的狀態(tài)方程；然后針對狀態(tài)方程設計狀態(tài)轉(zhuǎn)移矩陣，同時采用EKF設計Kalman濾波方程，并利用設計的Kalman濾波方程對含有雙頻彈性干擾下的角速度進行濾波處理。本發(fā)明適用于角速度濾波領(lǐng)域。
【IPC分類】G01P3/00
【公開號】CN105242058
【申請?zhí)枴緾N201510828799
【發(fā)明人】周荻, 鄒昕光, 張中磊, 朱蕊蘋
【申請人】哈爾濱工業(yè)大學
【公開日】2016年1月13日
【申請日】2015年11月24日