，yi表示i時(shí)刻的輸出；xi表示i時(shí)刻的輸入；yi-1表示(i-1)時(shí)刻的輸出；△T為采樣周期?；诨パa(bǔ)濾波的思想并結(jié)合離散化數(shù)字低通濾波器模型，構(gòu)造出如下模型：

其中，angle為系統(tǒng)最終的輸出角度值；gyr為由陀螺儀數(shù)據(jù)計(jì)算得出的角速度值，gyrdt則為一個(gè)采樣周期內(nèi)的角度增量；acc為由加速度計(jì)數(shù)據(jù)計(jì)算得出的角度值。當(dāng)選擇合適的時(shí)間常數(shù)RC和采樣周期△T時(shí)，此模型能模擬實(shí)現(xiàn)對(duì)陀螺儀的高通濾波和對(duì)加速度計(jì)的低通濾波
的互補(bǔ)組合功能。同時(shí)，時(shí)間常數(shù)和采樣周期也確定了系統(tǒng)對(duì)陀螺儀和加速度計(jì)輸出信號(hào)的置信度。當(dāng)本系統(tǒng)選擇時(shí)間常數(shù)為0．1 ms，采樣周期為5 ms時(shí)，代入上式模型有：

假設(shè)初始狀態(tài)時(shí)，系統(tǒng)角度估計(jì)值angle、陀螺儀輸出角速度值gyr和加速度計(jì)輸出的角度值acc均為零。表1是當(dāng)加速度計(jì)收到一個(gè)10°數(shù)值的突變信號(hào)時(shí)，在連續(xù)10個(gè)采樣周期內(nèi)估計(jì)值angle的變化過(guò)程。

從表中可以看出，在系統(tǒng)采樣周期設(shè)為5 ms時(shí)，如果此突變信號(hào)頻率高于200 Hz，即該信號(hào)持續(xù)時(shí)間不超過(guò)一個(gè)采樣周期，那么此信號(hào)給系統(tǒng)帶來(lái)的最大偏差不會(huì)超過(guò)0．9°。此時(shí)，可認(rèn)為系統(tǒng)對(duì)于加速度計(jì)的高頻信號(hào)不信任，而信任陀螺儀的信號(hào)。如果此突變信號(hào)頻率低于20 Hz，則該信號(hào)的持續(xù)時(shí)間會(huì)達(dá)到10個(gè)采樣周期以上，在第10個(gè)采樣周期時(shí)，系統(tǒng)的估計(jì)值angle為9．0°。此時(shí)，可認(rèn)為系統(tǒng)信任加速計(jì)的低頻信號(hào)。由此可見，此模型具有對(duì)加速度計(jì)的低通濾波功能和對(duì)陀螺儀的高通濾波功能。