1. 程式人生 > >025卡爾曼濾波中濾波增益與協方差陣的等價形式

025卡爾曼濾波中濾波增益與協方差陣的等價形式

  首先擺放一下前面的求解結果:

(1)Kk=Pk/k1HkT(HkPk/k1HkT+Rk)1 \tag{1} K_k = P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1}

(2)Pk=(IKkHk)Pk/k1 \tag{2} P_k = (I - K_k H_k ) P_{k/k-1}

Pk=(IKkHk)Pk/k1(IKkHk)T+KkRkKkT P_k = (I - K_kH_k) P_{k/k-1} (I - K_kH_k)^T + K_k R_k K_k^T\\

  公式(1)代入公式(2)展開的: (3)Pk=Pk/k1Pk/k1HkT(HkPk/k1HkT+Rk)1HkPk/k1=Pk/k1Pk/k1HkT(Rk+HkPk/k1HkT)1HkPk/k1 \tag{3} \begin{aligned} P_k &= P_{k/k-1} - P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1} H_k P_{k/k-1} \\ \\ &= P_{k/k-1} - P_{k/k-1} H_k^T( R_k + H_k P_{k/k-1} H_k^T )^{-1} H_k P_{k/k-1} \\ \end{aligned}

  比較矩陣求逆引理: (A+BCD)1=A1A1B(C1+DA1B)1DA1 (A+BCD)^{-1} = A^{-1} - A^{-1}B(C^{-1}+DA^{-1}B)^{-1} DA^{-1}   可將公式(3)轉換為:

(4)Pk=(Pk/k11+HkTRk1Hk)1 \tag{4} P_k = (P_{k/k-1}^{-1} + H_k^T R_k^{-1} H_k) ^{-1}

  根據矩陣求逆引理的方法構造公式(1):

(5)Kk=Pk/k1HkT(HkPk/k1HkT+Rk)1=[(HkPk/k1HkT+Rk)(HkT)1Pk/k11]1=[Hk+Rk(HkT)1Pk/k11]1=[Rk(HkT)1(HkTRk1Hk+Pk/k11)]1=(HkTRk1Hk+Pk/k11)1HkTRk1 \tag{5} \begin{aligned} K_k &= P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1}\\ \\ &= [(H_k P_{k/k-1} H_k^T + R_k)(H_k^T)^{-1} P_{k/k-1}^{-1}]^{-1}\\ \\ &= [H_k + R_k(H_k^T)^{-1} P_{k/k-1}^{-1}]^{-1}\\ \\ &= [R_k(H_k^T)^{-1}( H_k^T R_k^{-1} H_k + P_{k/k-1}^{-1})]^{-1}\\ \\ &= ( H_k^T R_k^{-1} H_k + P_{k/k-1}^{-1})^{-1}H_k^T R_k^{-1}\\ \end{aligned}

相關推薦

025濾波濾波增益等價形式

  首先擺放一下前面的求解結果: (1)Kk=Pk/k−1HkT(HkPk/k−1HkT+Rk)−1 \tag{1} K_k = P_{k/k-1} H_k^T(H_k P_{k/k-1} H_k^T + R_k)^{-1} Kk​=Pk/k−1​HkT​(Hk

PythonopenCV庫實現濾波案例

1.0 什麼是卡爾曼模型 理論推導過於複雜,看個知乎上的解釋吧  假設你有兩個感測器,測的是同一個訊號。可是它們每次的讀數都不太一樣,怎麼辦? 取平均。  再假設你知道其中貴的那個感測器應該準一些,便宜的那個應該差一些。那有比取平均更好的辦法嗎? 加權平均。  怎麼加權?假

無人駕駛四 擴充套件濾波在目標車輛運動狀態識別的運用(python程式)

# coding=utf-8 import numpy as np import numdifftools as nd import math dataset = [] # read the measurement data, use 0.0 to stand LI

基於濾波演算法在三維球軌跡跟蹤應用

關於卡爾曼濾波跟蹤演算法的理解文章實在太多,絕大多數都在敘述演算法原理和一些理解,而且一般舉例都限於一維直線運動或者二維平面運動,故在此不做過多的重複表述,有關原理理解性的文章請參考本部落格後的refe

濾波實現多項式擬合Matlab

nom and kalman ffi 樣本 矩陣 協方差 數組 fontsize %%%%%%%%%%%%%Q3:多項式系數估計%%%%%%%%%%%%%%%% %%%%%%%%%%2016/07/21%%%%%%%%%%%%%%%%%%% clc;clear;

濾波的一個簡單demo: 恒定加速度模型

obi vtt efk rtp end atp cee cdn bs4 p { margin-bottom: 0.1in; direction: ltr; color: rgb(0, 0, 10); line-height: 120%; text-align: left }

濾波3

比較 應該 想要 不能 模型 不同 隨著 而已 風景 假設你去一個風景區旅遊時候,種下一棵果樹,但顯然從果樹苗到果實不是一天兩天能完成對。需要慢慢長高。而你又不可能經常去看,但又想知道樹的高度,哪怎麽辦? 所以要解決的問題是:如何正確估計一棵果樹的高度? 我們想要知道的果

濾波2

推導 概率 精確 情況 多維 結果 width tex 為什麽 作者:肖暢鏈接:https://www.zhihu.com/question/23971601/answer/46480923來源:知乎著作權歸作者所有。商業轉載請聯系作者獲得授權,非商業轉載請註明出處。

濾波

nbsp 應用 一周 log -1 現在 richtext http 一是 假設我養了一只豬: 一周前,這只豬的體重是46±0.5kg。註意,在這裏我用了±0.5,表示其實我對這只豬一周前的體重並不是那麽確定的,也就是說,46k

濾波的原理說明

收集 引入 濾波 div 概率 一個人 pdf span net 在學習卡爾曼濾波器之前,首先看看為什麽叫“卡爾曼”。跟其他著名的理論(例如傅立葉變換,泰勒級數等等)一樣,卡爾曼也是一個人的名字,而跟他們不同的是,他是個現代人!卡爾曼全名Ru

【概率機器人】3.1 濾波、擴展濾波和無跡濾波

取出 嘗試 bar return tar 簡化 exp 回顧 clas 這一章將介紹卡爾曼濾波、擴展卡爾曼濾波以及無跡卡爾曼濾波,並從貝葉斯濾波的角度來進行分析並完成數學推導。如果您對貝葉斯濾波不了解,可以查閱相關書籍或閱讀 【概率機器人 2 遞歸狀態估計】。 這三種濾波方

MATLAB-濾波簡單運用示例

south noi 增加 jacob mage 影響 方差 sqrt mark 1、角度和弧度之間的轉換公式? 設角度為 angle,弧度為 radian radian = angle * pi / 180; angle = radian * 180 / pi; 所以在ma

cv2使用濾波(Kalman Filter)捕捉滑鼠運動

本文主要介紹在cv2中使用Kalman濾波捕捉滑鼠運動。 cv2.KalmanFilter(dynamParams=None,#狀態的維度 measureParams=None, #測量的維度 controlParams=None,#控制的維度 type=None)#矩陣的型別

濾波MATLAB程式碼實現

沒有大量的公式推導,個人感覺也沒有必要,我們從小推導過很多公式,試著想想我們還能回憶起幾個?個人認為只需要記住公式的用法,作用,知道有這個公式就可以。用的時候我們可以隨時去查。所以樓主參考網上資料結合一個小例子整理出卡爾曼濾波的MATLAB程式碼實現。看懂這些程式碼我們需要對卡爾曼濾波演算法有基本的

濾波的理解以及推導過程

針對的系統為: 狀態方程       X(k)=AX(k-1)+Bu(k-1)+W(k-1) 測量方程       Z(k)=HX(k)+V(k)  &

通俗理解濾波及其演算法實現(帶例項解析)

1.簡介(Brief Introduction) 在學習卡爾曼濾波器之前,首先看看為什麼叫“卡爾曼”。跟其他著名的理論(例如傅立葉變換,泰勒級數等等)一樣,卡爾曼也是一個人的名字,而跟他們不同的是,他是個現代人! 卡爾曼全名Rudolf Emil Kalman,匈牙利數學家,1930年出生於

初學者的卡濾波——擴充套件濾波

簡介 轉自:http://www.cnblogs.com/ymxiansen/p/5368547.html   已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點,相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用,但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著

詳解濾波

輕鬆一下看一個例子: 一片綠油油的草地上有一條曲折的小徑,通向一棵大樹。一個要求被提出:從起點沿著小徑走到樹下。 “很簡單。” A說,於是他絲毫不差地沿著小徑走到了樹下。 現在,難度被增加了:蒙上眼。 “也不難,我當過特種兵。” B說,於是他歪歪扭扭地走到了樹 ……

濾波的總結

現在可以關注用法,但後面要自己推導 1:卡爾曼增益就權值,權值的大小是由前一個時刻的表現來決定,誰的精度高誰的權值。 2:卡爾曼濾波是一個遞迴演算法,由前一時刻的預測值和當前時刻的測量值加權平均。理論上多次調整之後逐漸到達準確值。 3:濾波就是去掉不想要的訊號,保留有用的訊號。一般是去掉噪聲。 4:常