第11章 The Extended Kalman Filter 11.1 線性化卡爾曼濾波

• Linear Kalman Filter, 狀態(tài)方程

x ˙ = A x + B u + w z = H x + v \begin{aligned} \dot{\pmb{x}} &= \pmb{Ax}+\pmb{Bu}+\pmb{w} \\ \pmb{z} &= \pmb{Hx}+\pmb{v} \end{aligned} xxx˙zzz?=AxAxAx+BuBuBu+www=HxHxHx+vvv?

目前成都創(chuàng)新互聯(lián)已為上1000家的企業(yè)提供了網(wǎng)站建設、域名、虛擬主機、網(wǎng)站托管、服務器托管、企業(yè)網(wǎng)站設計、昌都網(wǎng)站維護等服務，公司將堅持客戶導向、應用為本的策略，正道將秉承"和諧、參與、激情"的文化，與客戶和合作伙伴齊心協(xié)力一起成長，共同發(fā)展。

• Extended Kalman Filter, 狀態(tài)方程

x = f ( x , u ) + w z = h ( x ) + ν \begin{aligned} \pmb{x} &= \pmb{f}(\pmb{x},\pmb{u})+\pmb{w}\\ \pmb{z} &= \pmb{h}(\pmb{x})+\pmb{\nu} \end{aligned} xxxzzz?=f?f??f(xxx,uuu)+www=hhh(xxx)+ννν?

F = ? f ( x t , u t ) ? x ∣ x t , u t H = ? h ( x ˉ t ) ? x ˉ ∣ x ˉ t \begin{aligned} \mathbf F &= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\ \mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} \end{aligned} FH?=?x?f(xt?,ut?)?∣∣∣∣?xt?,ut??=?xˉ?h(xˉt?)?∣∣∣∣?xˉt???

linear?Kalman?filter EKF F = ? f ( x t , u t ) ? x ∣ x t , u t x ˉ = F x + B u x ˉ = f ( x , u ) P ˉ = F P F T + Q P ˉ = F P F T + Q H = ? h ( x ˉ t ) ? x ˉ ∣ x ˉ t y = z ? H x ˉ y = z ? h ( x ˉ ) K = P ˉ H T ( H P ˉ H T + R ) ? 1 K = P ˉ H T ( H P ˉ H T + R ) ? 1 x = x ˉ + K y x = x ˉ + K y P = ( I ? K H ) P ˉ P = ( I ? K H ) P ˉ \begin{array}{l|l} \text{linear Kalman filter} & \text{EKF} \\ \hline & \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\ \mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)} \\ \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\ \hline & \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\ \textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\ \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\ \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\ \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} \end{array} linear?Kalman?filterxˉ=Fx+BuPˉ=FPFT+Qy=z?HxˉK=PˉHT(HPˉHT+R)?1x=xˉ+KyP=(I?KH)Pˉ?EKFF=?x?f(xt?,ut?)?∣∣∣∣?xt?,ut???xˉ=f(x,u)?Pˉ=FPFT+QH=?xˉ?h(xˉt?)?∣∣∣∣?xˉt???y=z?h(xˉ)?K=PˉHT(HPˉHT+R)?1x=xˉ+KyP=(I?KH)Pˉ??

11.2 實例

背景：1）考慮二維平面；2）Aircraft只有水平方向的速度；3）Aircraft高度假設不變；4）Radar隨著距離的變大，精度減低

• 下面這個是Radar的模型：

class Radar:
    """模擬雷達傳感器(二維平面)"""

    def __init__(self, dt, pos, vel, alt):
        """
        Parameters
        ----------
        dt : double
            采樣時間
        pos : double
            水平距離
        vel : double
            水平速度
        alt : double
            垂直高度
        Returns
        -------
        None.
        """

        # 初始化Radar
        self.dt = dt
        self.pos = pos
        self.vel = vel
        self.alt = alt

    def get_range(self):
        """得到物體當前運動信息"""

        # 添加一些噪聲
        self.vel = self.vel + .1*randn()
        self.alt = self.alt + .1*randn()
        # 從上一刻的位置獲得當前時刻的位置
        self.pos = self.pos + self.vel*self.dt

        # 假設雷達的精度隨著距離變遠而越來越低
        err = self.pos * (0.05*randn())
        dist = math.sqrt(self.pos**2 + self.alt**2) + err

        return dist

下面說明系統(tǒng)的狀態(tài)方程和測量方程，首先得確定系統(tǒng)的狀態(tài)變量 x = ( x 0 x 1 x 2 ) T \pmb{x} = \begin{pmatrix} x_0 & x_1 & x_2 \end{pmatrix}^T xxx=(x0??x1??x2??)T

x 0 {x}_0 x0?：水平距離。 x 1 {x}_1 x1?：水平速度。 x 2 {x}_2 x2?：垂直高度。

• 系統(tǒng)狀態(tài)方程：（需要注意，速度只有水平方向，所以 x ˙ 0 = x 1 \dot{x}_0=x_1 x˙0?=x1?）

[ x ˙ 0 x ˙ 1 x ˙ 2 ] = [ 0 1 0 0 0 0 0 0 0 ] [ x 0 x 1 x 2 ] + w \begin{bmatrix} \dot{x}_0 \\ \dot{x}_1 \\ \dot{x}_2 \\ \end{bmatrix} =\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ \end{bmatrix} + \pmb{w} ???x˙0?x˙1?x˙2?????=???000?100?000???????x0?x1?x2?????+www

系統(tǒng)的狀態(tài)方程是連續(xù)的線性的，將其離散化（對確定性部分）：

F = e A d t ≈ I + A d t \begin{aligned} \pmb{F} &= e^{\pmb{A}dt} \\ &\approx \pmb{I} + \pmb{A}dt \end{aligned} FFF?=eAAAdt≈III+AAAdt?

所以離散化后的狀態(tài)方程有：(其實這里還有一個問題， w \pmb{w} www變成什么了？）

x ( k ) = [ 1 d t 0 0 1 0 0 0 1 ] x ( k ? 1 ) \pmb{x}(k)= \begin{bmatrix} 1 & dt & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \pmb{x}(k-1) xxx(k)=???100?dt10?001????xxx(k?1)

• 測量方程
  z = h ( x ) + v = x 0 2 + x 2 2 + v \begin{aligned} \pmb{z} &= \pmb{h}(\pmb{x}) + \pmb{v} \\ &= \sqrt{x_{0}^{2}+x_{2}^{2}} + \pmb{v} \\ \end{aligned} zzz?=hhh(xxx)+vvv=x02?+x22? ?+vvv?
  離散化后，測量方程為；
  z k = [ x 0 x 0 2 + x 2 2 0 x 2 x 0 2 + x 2 2 ] x k \pmb{z}_k= \begin{bmatrix} \frac{x_0}{x_0^2+x_2^2} & 0 & \frac{x_2}{x_0^2+x_2^2} \end{bmatrix} \pmb{x}_k zzzk?=[x02?+x22?x0???0?x02?+x22?x2???]xxxk?

這里有幾點需要注意：

1. 特別是隨機噪聲 Q , R \pmb{Q},\pmb{R} Q?Q??Q,RRR，需要根據(jù)實際的情況進行不斷的調(diào)整
2. 初始值的設定具有很大的隨機性， x 0 , P 0 \pmb{x}_0,\pmb{P}_0 xxx0?,PPP0?幾乎沒有什么影響。

關于這一點可以參考一篇文獻，嚴恭敏老師的《傳統(tǒng)組合導航中的實用Kalman濾波技術評述》，給出了很多實用的建議：1）只要濾波器是漸近穩(wěn)定，初值不會影響估計的最終結果（和系統(tǒng)的可觀性有一定的關系）；2）去精確的離散化噪聲的協(xié)方差矩陣，還不如增加采樣頻率所提高的精度多。

• 仿真結果（及其調(diào)試的過程記錄）
  • 協(xié)方差矩陣的更新公式，在代碼中的實現(xiàn)出現(xiàn)了錯誤。/_ \
    
    
    
    
    
  • 然后改到正確的公式后，它竟然還是發(fā)散的X_X

問題分析：1）在初始值的估計值還是沒有偏差的情況下進行的。。2）

解決辦法：1）查看kalman濾波是否正確表達了；2）將初始值調(diào)為有一定偏差的，看是否有其它的問題；3)增加計算的時間后，又出現(xiàn)了NaN的數(shù)據(jù)，看來電腦的計算還是沒有搞清楚！

結果：發(fā)現(xiàn)對于Numpy的數(shù)值的維度有點混亂，導致矩陣的計算出現(xiàn)了些許問題?，F(xiàn)在已經(jīng)有點暈了，需要重新再來一遍（遇到了一點問題，于是就又想逃避了哈？？還是先吃飯吧）

(python numpy) np.array.shape 中 (3,)、(3,1)、(1,3)的區(qū)別_我是一顆棒棒糖的博客-博客_np.array.shape

這里在強調(diào)一下，需要注意一下這個的運算過程，看看是否對于np.dot有什么誤解呢。（突然好想用matlab啊，這個python是真的┭┮﹏┭┮）

numpy還有數(shù)組和矩陣的區(qū)別，我是真的醉醉的呢啊，下面我該怎么辦？(就用數(shù)組類型了，現(xiàn)在就是要搞懂數(shù)組的運算，當然剛才維數(shù)這里也搞錯了一點問題，繼續(xù)加深理解唄）

• 經(jīng)過仔細的調(diào)整，主要就是numpy的narray的運算進行了詳細的對比，現(xiàn)在的結果稍微好了一點點呢?。ㄟ@里對于狀態(tài)的初始值也進行了調(diào)整和容易看出來的）

現(xiàn)在這里還有一個很明顯的問題，速度的收斂速度好像有點慢，而且最后對于高度的估計有發(fā)散的趨勢嗎？（延長一點時間看看？）
然后它它，就直接起飛?。ǚ治鲈?，因為隨著距離的增加，radar的噪聲逐漸增大的，然后就不能正確的估計啦？后面調(diào)試后確實是的）



一個這個程序，竟然調(diào)了兩天，我？？？

分析原因：

1. EKF還是不太熟悉，連公式都打錯
2. 第一次用Python來寫，對于一些基本的運算不太了解，導致很多錯誤的都是計算的形式有問題
3. 遇到問題的心態(tài)要好好調(diào)整，哪里不會就點哪個（人生苦短，不用什么都要學透）

參考資料：

Kalman-and-Bayesian-Filters-in-Python

你是否還在尋找穩(wěn)定的海外服務器提供商？創(chuàng)新互聯(lián)www.cdcxhl.cn海外機房具備T級流量清洗系統(tǒng)配攻擊溯源，準確流量調(diào)度確保服務器高可用性，企業(yè)級服務器適合批量采購，新人活動首月15元起，快前往官網(wǎng)查看詳情吧

當前題目：第11章TheExtendedKalmanFilter-創(chuàng)新互聯(lián)
文章網(wǎng)址：http://www.rwnh.cn/article4/cssdie.html

成都網(wǎng)站建設公司_創(chuàng)新互聯(lián)，為您提供域名注冊、建站公司、App開發(fā)、移動網(wǎng)站建設、虛擬主機、做網(wǎng)站

廣告

聲明：本網(wǎng)站發(fā)布的內(nèi)容（圖片、視頻和文字）以用戶投稿、用戶轉載內(nèi)容為主，如果涉及侵權請盡快告知，我們將會在第一時間刪除。文章觀點不代表本網(wǎng)站立場，如需處理請聯(lián)系客服。電話：028-86922220；郵箱：631063699@qq.com。內(nèi)容未經(jīng)允許不得轉載，或轉載時需注明來源： 創(chuàng)新互聯(lián)