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)站建設(shè)、域名、虛擬主機(jī)、網(wǎng)站托管、服務(wù)器托管、企業(yè)網(wǎng)站設(shè)計(jì)、昌都網(wǎng)站維護(hù)等服務(wù)，公司將堅(jiān)持客戶導(dǎo)向、應(yīng)用為本的策略，正道將秉承"和諧、參與、激情"的文化，與客戶和合作伙伴齊心協(xié)力一起成長，共同發(fā)展。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 實(shí)例
背景：1）考慮二維平面；2）Aircraft只有水平方向的速度；3）Aircraft高度假設(shè)不變；4）Radar隨著距離的變大，精度減低
重慶分公司
重慶分公司
class Radar:
"""模擬雷達(dá)傳感器(二維平面)"""
def __init__(self, dt, pos, vel, alt):
"""
Parameters
----------
dt : double
采樣時(shí)間
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):
"""得到物體當(dāng)前運(yùn)動(dòng)信息"""
# 添加一些噪聲
self.vel = self.vel + .1*randn()
self.alt = self.alt + .1*randn()
# 從上一刻的位置獲得當(dāng)前時(shí)刻的位置
self.pos = self.pos + self.vel*self.dt
# 假設(shè)雷達(dá)的精度隨著距離變遠(yuǎn)而越來越低
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?：垂直高度。
[ 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)方程有：(其實(shí)這里還有一個(gè)問題， 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)
這里有幾點(diǎn)需要注意：
關(guān)于這一點(diǎn)可以參考一篇文獻(xiàn)，嚴(yán)恭敏老師的《傳統(tǒng)組合導(dǎo)航中的實(shí)用Kalman濾波技術(shù)評述》，給出了很多實(shí)用的建議：1）只要濾波器是漸近穩(wěn)定，初值不會(huì)影響估計(jì)的最終結(jié)果（和系統(tǒng)的可觀性有一定的關(guān)系）；2）去精確的離散化噪聲的協(xié)方差矩陣，還不如增加采樣頻率所提高的精度多。
問題分析：1）在初始值的估計(jì)值還是沒有偏差的情況下進(jìn)行的。。2）
解決辦法：1）查看kalman濾波是否正確表達(dá)了；2）將初始值調(diào)為有一定偏差的，看是否有其它的問題；3)增加計(jì)算的時(shí)間后，又出現(xiàn)了NaN的數(shù)據(jù)，看來電腦的計(jì)算還是沒有搞清楚！
結(jié)果：發(fā)現(xiàn)對于Numpy的數(shù)值的維度有點(diǎn)混亂，導(dǎo)致矩陣的計(jì)算出現(xiàn)了些許問題?，F(xiàn)在已經(jīng)有點(diǎn)暈了，需要重新再來一遍（遇到了一點(diǎn)問題，于是就又想逃避了哈？？還是先吃飯吧）
(python numpy) np.array.shape 中 (3,)、(3,1)、(1,3)的區(qū)別_我是一顆棒棒糖的博客-博客_np.array.shape
這里在強(qiáng)調(diào)一下，需要注意一下這個(gè)的運(yùn)算過程，看看是否對于np.dot有什么誤解呢。（突然好想用matlab啊，這個(gè)python是真的┭┮﹏┭┮）
numpy還有數(shù)組和矩陣的區(qū)別，我是真的醉醉的呢啊，下面我該怎么辦？(就用數(shù)組類型了，現(xiàn)在就是要搞懂?dāng)?shù)組的運(yùn)算，當(dāng)然剛才維數(shù)這里也搞錯(cuò)了一點(diǎn)問題，繼續(xù)加深理解唄）
現(xiàn)在這里還有一個(gè)很明顯的問題，速度的收斂速度好像有點(diǎn)慢，而且最后對于高度的估計(jì)有發(fā)散的趨勢嗎？（延長一點(diǎn)時(shí)間看看？）
然后它它，就直接起飛?。ǚ治鲈?，因?yàn)殡S著距離的增加，radar的噪聲逐漸增大的，然后就不能正確的估計(jì)啦？后面調(diào)試后確實(shí)是的）
一個(gè)這個(gè)程序，竟然調(diào)了兩天，我？？？
分析原因：
Kalman-and-Bayesian-Filters-in-Python
你是否還在尋找穩(wěn)定的海外服務(wù)器提供商？創(chuàng)新互聯(lián)www.cdcxhl.cn海外機(jī)房具備T級流量清洗系統(tǒng)配攻擊溯源，準(zhǔn)確流量調(diào)度確保服務(wù)器高可用性，企業(yè)級服務(wù)器適合批量采購，新人活動(dòng)首月15元起，快前往官網(wǎng)查看詳情吧
