卡曼濾波器入門教程α?β?γ濾波器 2

估計算法

下圖描述了本示例中使用的估計算法：

現(xiàn)在我們準備開始測量和估計過程。

迭代0：

初始化：

我們最初猜測金條的重量是1000克，初始猜測只進行一次且僅用于濾波器初始化階段，因此，后續(xù)迭代過程中不需要它。

=1000g

預測：

金條的重量不應該改變，因此，系統(tǒng)的動態(tài)模型是靜態(tài)的，我們的下一個狀態(tài)估計（預測）等于初始化狀態(tài)：

迭代1：

步驟1：

用天平秤對金條進行測量：

Z1=1030g

步驟2：

計算增益，在我們的示例中，αn=1/n，因此：

α1=1/1=1

使用狀態(tài)更新方程計算當前估計：

=

+α1(z1?

)=1000+1(1030?1000)=1030g

注意: 本示例中，初始估計值可以是任何值，因為α1=1，初始估計值會在第一輪迭代中被消除。

步驟3：

系統(tǒng)是靜態(tài)的；因此，金條的重量不應該改變，我們的下一狀態(tài)估計（預測）等于當前狀態(tài)估計：

=

=1030g

迭代2：

在完成一個時間周期后，來自上一次迭代的預測估計變?yōu)楫斍暗械纳弦淮喂烙嫞?/p>

=1030g

步驟1：

進行第二次重量測量：

z2=989g

步驟2：

計算增益：

α2=1/2

計算當前估計：

=

+α2(z2?

)=1030+1/2(989?1030)=1009.5g

步驟3：

=

=1009.5g

迭代3：

z3=1017g α3=1/3

= 1009.5+13(1017?1009.5)=1012g

=1012g

迭代10：

z10=1011g α10=1/10

=1011+110(1011?1011)=1011gx^10,10=1011+110(1011?1011)=1011g

到這我們就可以停止了，增益隨著每次測量而減小，因此，后續(xù)每次測量的貢獻低于前一次測量的貢獻，我們已經(jīng)非常接近真實重量，即1010克。如果我們進行更多的測量，我們會更接近真實值。

下表對我們的測量值和估計值進行了總結(jié)，圖表對測量值、估計值和真實值進行了比較。

該估計算法對測量值具有平滑效果，并向真實值收斂。

在這個例子中，我們?yōu)殪o態(tài)系統(tǒng)開發(fā)了一個簡單的狀態(tài)估計算法，我們還導出了狀態(tài)更新方程，這是五個卡爾曼濾波方程之一。

示例2：跟蹤一維空間下的勻速飛行器

是時候研究一個隨時間改變狀態(tài)的動態(tài)系統(tǒng)了。在本例中，我們使用α-β濾波器跟蹤勻速飛機的一維狀態(tài)。

讓我們假設(shè)一個一維世界，一架飛機正在徑向遠離雷達（或朝向雷達）移動，在一維世界中，雷達的角度是恒定的，飛機的高度也是恒定的。

xn表示時間n時飛機的航程，飛機速度可以使用航程微分法來近似，即測量航程隨時間的變化。

因此，速度是航程的導數(shù)：

雷達以恒定速率向目標方向發(fā)送跟蹤波束，時間間隔為Δt，下面兩個運動方程可以描述系統(tǒng)勻速運動的動力學模型：

根據(jù)上面的方程，下一追蹤周期的飛機航程等于當前追蹤周期航程加上目標速度乘以追蹤間隔，另外，由于我們在本例中假設(shè)速度恒定，因此，下一個周期的速度等于當前周期的速度。

上述方程組稱為狀態(tài)推導方程（也稱為轉(zhuǎn)換方程或預測方程），也是五個卡爾曼濾波方程之一，這個方程組將通過當前狀態(tài)推導得到下一狀態(tài)（預測）。

我們在上一個示例中也使用了狀態(tài)推導方程，其中我們假設(shè)下一個周期的黃金重量等于當前周期的黃金重量。

狀態(tài)推導方程取決于當前是什么系統(tǒng)，并因系統(tǒng)類型而異，狀態(tài)推導方程可以使用矩陣進行表示，我們稍后再學習。

注意: 我們已經(jīng)學習了五個卡曼濾波方程中的兩個:?狀態(tài)更新方程狀態(tài)推到方

現(xiàn)在，我們將修改本示例的狀態(tài)更新方程。

α-β濾波器

讓雷達進行跟蹤（Δt周期為5秒），假設(shè)在時間n?1無人機的估計航程為30000m，估計無人機速度為40m/s。

使用狀態(tài)推導方程，我們可以預測時間n時的目標位置：

對于時間n的目標速度預測：

然而，在時間n，雷達測量的航程（zn）為30110米，而不是預期的30200米，預期（預測）值和測量值之間有90m的差距，造成這種差距的原因有兩種：

l雷達測量不準確

l飛行器速度變了，新速度為：（30110-30200）/5 = 22m/s

上面哪一個假設(shè)是正確的？

讓我們寫下速度的狀態(tài)更新方程：

因子β的值取決于雷達的精度水平，假設(shè)雷達的1σ精度為20m，那么，預測距離和測量距離之間的90米差距很可能是飛機速度變化造成的，在這種情況下，我們應該將β因子設(shè)置為高值，如果我們設(shè)置β=0.9，則估計速度為：

同理，假設(shè)雷達的1σ精度為150米，那么90米的距離偏差可能是雷達測量誤差造成的。在這種情況下，我們應該將β因子設(shè)置為較低的值，如果我們設(shè)置β=0.1，則估計速度為：

如果飛機速度真的從40m/s變?yōu)?2m/s，我們可以在10個追蹤周期后發(fā)現(xiàn)它（以β=0.1運行上述等式10次）。如果差距是由測量誤差引起的，則后續(xù)的測量值將在真實值上下輕微浮動，因此，平均而言，目標速度不會改變。

飛機位置的狀態(tài)更新方程類似于前一示例中導出的方程：

與前面的例子不同，在每次迭代中都會重新計算α因子（αn=1/N），在本例中α因子是常數(shù)。

α因子的大小取決于雷達測量精度。對于高精度雷達，我們應該選擇高α，給測量值賦予高權(quán)重，如果α=1，則估計值將等于測量值：

如果α=0，則測量值沒有意義：

因此，我們推導出了一個方程組，它構(gòu)成了雷達跟蹤器的狀態(tài)更新方程，它們也被稱為α?β軌跡更新方程或α?β軌跡濾波方程。

注意: 在某些書籍中, α?β濾波器也被叫做g-h濾波器

注意: 在本例中，我們從航程測量值得出飛機速度，現(xiàn)代雷達可以利用多普勒效應直接測量徑向速度，然而，我的目標是解釋卡爾曼濾波器，而不是雷達操作。因此，為了通用性，我將繼續(xù)從我們的示例中的距離測量中推導出速度。

下圖描述了本示例中使用的估計算法：

與前一示例不同，本示使用了增益值α和β，在卡爾曼濾波器中，α和β被卡爾曼增益代替，卡爾曼增益是在每次迭代時計算得到的，我們稍后才了解。