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

估計算法

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

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

迭代0：

初始化：

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

=1000g

預(yù)測：

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

迭代1：

步驟1：

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

Z1=1030g

步驟2：

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

α1=1/1=1

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

=

+α1(z1?

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

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

步驟3：

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

=

=1030g

迭代2：

在完成一個時間周期后，來自上一次迭代的預(yù)測估計變?yōu)楫?dāng)前迭代中的上一次估計：

=1030g

步驟1：

進行第二次重量測量：

z2=989g

步驟2：

計算增益：

α2=1/2

計算當(dāng)前估計：

=

+α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)估計算法，我們還導(dǎo)出了狀態(tài)更新方程，這是五個卡爾曼濾波方程之一。

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

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

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

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

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

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

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

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

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

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

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

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

α-β濾波器

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

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

對于時間n的目標(biāo)速度預(yù)測：

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

l雷達測量不準(zhǔn)確

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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