多旋翼無人機遠程控制實踐 課件ch05

濾波器設計實驗多旋翼無人機遠程控制實踐第5章01實驗原理卡爾曼濾波器算法總結(jié)步驟一:過程模型觀測模型步驟二:初始狀態(tài)卡爾曼濾波器算法總結(jié)步驟三:當k=0時，。步驟四:k=k+1。步驟五:狀態(tài)估計預測步驟六:誤差協(xié)方差預測卡爾曼濾波器算法總結(jié)步驟七:卡爾曼增益矩陣步驟八:狀態(tài)估計更新步驟九:誤差協(xié)方差更新卡爾曼濾波器算法總結(jié)步驟十:返回步驟四?？柭鼮V波器算法總結(jié)(1)卡爾曼濾波器在進行濾波器估計的同時還產(chǎn)生了誤差協(xié)方差陣，它用于表征估計精度，同時也能用于傳感器的健康評估。(2)一般來說，采樣周期合理情況下，連續(xù)系統(tǒng)可觀，離散化后的系統(tǒng)也會可觀然而采樣周期選擇不當，系統(tǒng)可能失去可控性及可觀性。因此，原則上應該檢查離散化系統(tǒng)的可觀性?？柭鼮V波器算法總結(jié)(3)要求非奇異否則表達式

沒有意義。一般情況下Rk非奇異，即說明觀測信號總是存在一定噪聲，因此

總是非奇異的。(4)如果系統(tǒng)

不可觀，卡爾曼波器仍然可以運行，只是不可觀的模態(tài)沒有進行更新，僅執(zhí)行遞推步驟?？紤]極端的情況，如果，即整個系統(tǒng)完全不可觀，則

，意味著。02基礎(chǔ)實驗(1)學習并掌握卡爾曼濾波器算法的原理及應用。(2)在仿真1.0中，將輸入的真實信號與卡爾曼波后的信號進行對比，比較卡爾曼濾波器在不同參數(shù)下的效果。(3)在仿真2.0中，加入卡爾曼濾波器，同時對比仿真1.0與仿真2.0的濾波效果。(4)為了簡便，本書的輸入信號沒有引入噪聲。目標實驗目標步驟一:學習軟件環(huán)境學習本書第2章的內(nèi)容，對于Simulink模型有一個初步的了解。步驟二:初始化參數(shù)打開本章所附帶文件夾中的“e2\e2.1\sim1.0”文件夾，打開“e2\e2.1\sim1.0\start-Simulation.m”文件，并單擊“運行”按鈕，初始化參數(shù)。仿真1.0實驗步驟步驟三:濾波器模型打開“e2\e2.1\sim1.0\e21TFKalmanFilter.slx”模型文件，將看到多旋翼Simulink仿真平臺，詳細內(nèi)容請參考第2章2.2.1節(jié)，如圖5.2所示。圖5.3中的虛線框部分為速度獲取的模塊，采取卡爾曼濾波的方法進行速度和偏航角速率的實時獲取。打開任意一個模塊，例如水平前向通道模塊，其內(nèi)部如圖5.4所示。仿真1.0實驗步驟實驗步驟實驗步驟實驗步驟步驟四:重新初始化參數(shù)在完成了相應的修改后，可以進行實驗。開始時已經(jīng)運行過“startSimulationm”文件，如果改動該文件中的參數(shù)，則需要在MATLAB中重新運行該文件。接下來，只需在Simulink中運行模型文件“e2\e2.1\sim1.0\e21TFKalanFilter.slx”即可。仿真1.0實驗步驟實驗步驟實驗步驟實驗步驟步驟五:觀察濾波效果觀察卡爾曼濾波的效果，將每個通道的速度獲取模塊中的開關(guān)撥至下部，使得閉環(huán)反饋中采用通過卡爾曼濾波器的反饋值。實驗步驟步驟六:與真實輸入做對比步驟一:打開命名為“e2\e2.1\sim2.0”的文件夾，其中的文件與上面的“e2\e2.1\sim1.0”文件夾相同，而區(qū)別在于文件“e2\e2.1\sim2.0”中的模型為非線性模型。步驟二:采用與傳遞函數(shù)模型實驗中完全相同的實驗步驟，觀察獲得結(jié)果，并將兩種模型的關(guān)鍵結(jié)果進行對比。仿真2.0實驗步驟實驗步驟03分析實驗實驗步驟步驟一：測量噪聲在位置信號測量的過程中加入噪聲，觀察濾波器對噪聲的濾波效果，加入Simulink自帶的隨機噪聲模塊如圖5.9中的虛線框所示。仿真1.0實驗步驟加入噪聲之后，分別在多旋翼各通道進行控制，使其穩(wěn)定運行，利用示波器觀察噪聲濾波效果。為了方便起見，本書將各通道的噪聲進行統(tǒng)一設定，并進行量化。仿真1.0實驗步驟步驟二:調(diào)整卡爾曼濾波器中的測量噪聲方差陣和過程噪聲方差陣仿真1.0實驗步驟(1)噪聲增益“gainnoise=0.25”。高度通道真實速度與濾波速度對比圖以及在不同卡爾曼濾波器測量噪聲標準差下濾波速度對比圖分別如圖5.10和圖5.11所示，其中每組對應的參數(shù)如表5.5所示。實驗步驟實驗步驟實驗步驟仿真1.0(2)噪聲增益“gainnoise1”。高度通道真實速度與濾波速度對比圖以及在不同卡爾曼濾波器測量噪聲標準差下濾波速度對比圖分別如圖5.12和圖5.13所示，其中每組對應的參數(shù)如表5.6所示。實驗步驟仿真1.0實驗步驟仿真1.0實驗步驟(3)噪聲增益“gainnoise-3”高度通道真實速度與濾波速度對比圖以及在不同卡爾曼濾波器測量噪聲標準差下濾波速度對比圖分別如圖5.14和圖5.15所示，其中每組對應的參數(shù)如表5.7所示。仿真1.0實驗步驟仿真1.0實驗步驟仿真1.0實驗步驟(4)小結(jié)從上面實驗結(jié)果可以看出，在某一噪聲值下，隨著濾波器的測量噪聲標準差的提高系統(tǒng)中的測量噪聲被逐漸濾掉，可以認為卡爾曼濾波器是非常有效的。在調(diào)整了噪聲系數(shù)并獲得最佳效果之后，濾波結(jié)果變得更平滑了，噪聲幾乎被消除。對比三種噪聲下的最優(yōu)參數(shù)，如表5.8所示。實驗步驟仿真2.0(1)對于在傳遞函數(shù)模型中已經(jīng)完成的實驗過程，需要在非線性模型上即在仿真2.0中進行驗證。(2)打開命名為“e2\e2.2\sim2.0”的文件夾，其中的文件與上面的“e2\e2.2\sim1.0文件夾相同，而區(qū)別在于“e2\e2.2\sim2.0”文件夾中的模型為非線性模型。(3)采用與傳遞函數(shù)模型實驗中完全相同的實驗步驟，觀察獲得結(jié)果，并將兩種模型的關(guān)鍵結(jié)果進行對比。04設計實驗實驗設計(1)預測第k-2步的狀態(tài)及誤差協(xié)方差：(2)更新第k-2步的狀態(tài)及誤差協(xié)方差：方法一:遞推預測法實驗設計(3)預第步的狀態(tài)及誤差協(xié)方差：方法一:遞推預測法實驗設計方法二：推廣法對狀態(tài)方程進行整理得到：實驗設計方法二：推廣法令狀態(tài)變量為實驗設計方法二：推廣法因為經(jīng)過整理后的模型與經(jīng)典的模型一致，所以可以直接套用遞推公式，整理后模型的噪聲方差陣變?yōu)橄聢D。實驗設計則遞推過程如下。(1)預測第k步的狀態(tài)及誤差協(xié)方差：(2)更新第k步的狀態(tài)及誤差協(xié)方差：實驗步驟具體操作步驟與基礎(chǔ)實驗的類似，詳細步驟請參考5.2.2.1節(jié)。不同的是，本實驗由于考慮了延時的影響，因而對圖5.4中卡爾曼濾波器結(jié)構(gòu)做了修改，修改后的的濾波器如圖5.17所示。這里主要在位置信號輸入添加了兩個延時模塊。仿真1.0實驗步驟實驗步驟實驗步驟(1)對于在傳遞函數(shù)模型中已經(jīng)完成的實驗過程需要在非線性模型上在仿真20中進行驗證。(2)打開命名為“e2\e2.3\sim2.0”的文件夾，其中的文件與上面的“e2\e2.3\sim1.0”文件夾相同，而區(qū)別在于“e2\e2.3\sim2.0”中的模型為非線性模型。(3)采用與傳遞函數(shù)模型實驗中完全相同的實驗步驟，觀察獲得結(jié)果，并將兩種模型的關(guān)鍵結(jié)果進行對比。仿真2.0實驗步驟實驗步驟硬件在環(huán)仿真實驗步驟硬件在環(huán)仿真實驗步驟硬件在環(huán)仿真05實飛實驗實驗步驟具體步驟了解辨識相關(guān)流程打開濾波模型設置正弦輸入初始化參數(shù)系統(tǒng)啟動流程設置參數(shù)，觀察實驗效果對比效果觀察實驗步驟步驟二實驗步驟步驟二實驗步驟步驟二實驗步驟步驟二實驗步驟結(jié)果及分析在真實情況下，觀測信息含有一定延時，因而需要考慮延時情況下的卡爾曼濾波，可以參考設計實驗中的遞推預測法和擴維法。從實飛實驗對比的結(jié)果可以看出，即使是在有延時情況下，卡爾曼濾波仍能有效獲得速度信號并且結(jié)果與基礎(chǔ)實驗中的速度獲取結(jié)果相似。綜上所述，在有延時情況下卡爾曼濾波器仍然可以被使用。實驗步驟結(jié)果及分析實驗步驟實飛實驗高度通道卡爾曼濾波器控制效果圖如圖5