2. 程式人生 > >無損卡爾曼濾波UKF與多感測器融合

無損卡爾曼濾波UKF與多感測器融合

阿新 • • 發佈：2019-01-09

這裡寫圖片描述

非線性系統狀態估計是一大難點。KF（Kalman Filter）只適用於線性系統。EKF（Extended Kalman Filter）利用泰勒展開將非線性系統線性化。可是，EKF在強非線性系統下的誤差很大。本文將介紹一種新型的濾波演算法UKF（Unscented Kalman Filter），其計算精度相比EKF更高並省略了Jacobian矩陣的計算。

Why UKF

這裡寫圖片描述

本部落格在之前兩篇介紹了KF和EKF。那麼，為什麼還需要UKF呢，原因見下表：

模型 缺點 UKF對缺點改進
KF 只適用於線性系統 適用於非線性系統
EKF 線性化忽略了高階項導致強非線性系統誤差大；線性化處理需要計算Jacobian矩陣 對非線性的概率分佈近似，沒有線性化忽略高階項； 不需要計算Jacobian矩陣

UKF簡述

原理概述

首先，回顧下UKF需要解決的問題，已知系統的狀態及其方差xk,Pk。如果經過非線性函式xk+1=f(xk)後，新的狀態和方差如何求解。

EKF提供的方法是將非線性函式f(x)作泰勒一階展開，利用Jacobian矩陣Hj近似將f(x)線性化為Hjx。這種方法一方面在強非線性系統下誤差大，另一方面Jacobian矩陣的計算著實令人頭疼。

UKF認為每一個狀態xk,Pk都可以用幾個Sigma點（關鍵點）Xsig表示。當作用於非線性函式f(x)時，只需要將Sigma點Xs

ig作用於非線性函式f(x)得到f(Xsig)即可。通過得到的f(Xsig)可以計算新的狀態xk+1,Pk+1。

這裡寫圖片描述

通過上面的介紹，我們知道UKF只是將非線性函式對映通過關鍵點對映來實現，那麼出現幾個問題：

  1. 關鍵點怎麼找
  2. 找到關鍵點後如何求出新的狀態xk+1,Pk+1

關鍵點怎麼找

關鍵點的意義在於能夠充分刻畫原狀態的分佈情況，其經驗公式如下圖所示，需要注意的是：

  • nx代表xk|k的大小
  • λ代表關鍵點的散開情況，一般採用經驗值λ=3−nx

這裡寫圖片描述

找到關鍵點後如何求出新的狀態

新狀態的求解公式如下圖所示，需要注意的是：

  • Xk+1|k代表Sigma點集合，Xk+1|k,i
    代表Sigma點集合中的第i個點
  • na代表xk+1|k增廣後的大小
  • nσ代表Sigma點集合的大小，一般nσ=1+2na
  • 權重wi在i==0時w0=λλ+na，在i!=0時w0=12(λ+na)

這裡寫圖片描述

多感測器融合

下面，將通過lidar、radar跟蹤小車的例子，講解UKF如何應用於小車狀態跟蹤。相關感測器資訊及大體步驟可見擴充套件卡爾曼濾波EKF與多感測器融合。

CTRV模型

EKF文章中使用了CV（constant velocity）模型，本文將使用CTRV（constant turn rate and velocity magnitude）模型。其狀態變數如下圖所示。

這裡寫圖片描述

因假定turn rate、velocity不變，其Process noise包含加速度與角加速度為：

νk=[νa,kνψ¨,k]

利用x˙及其對時間的積分xk+1=∫x˙dt可得Process模型為：

xk+1=xk+⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢vkψk˙(sin(ψk

相關推薦

無損卡爾曼濾波UKF與多感測器融合

非線性系統狀態估計是一大難點。KF（Kalman Filter）只適用於線性系統。EKF（Extended Kalman Filter）利用泰勒展開將非線性系統線性化。可是，EKF在強非線性系統下的誤差很大。本文將介紹一種新型的濾波演算法UKF（Unscen

擴充套件卡爾曼濾波EKF與多感測器融合

Extended Kalman Filter（擴充套件卡爾曼濾波）是卡爾曼濾波的非線性版本。在狀態轉移方程確定的情況下，EKF已經成為了非線性系統狀態估計的事實標準。本文將簡要介紹EKF，並介紹其在無人駕駛多感測器融合上的應用。 KF與EKF 本文假定讀

無損卡爾曼濾波UKF(3)-預測-生成Sigma點

# 無損卡爾曼濾波UKF（3）-預測-生成Sigma點 ## 1 選擇建立Sigma點 ### A 根據 已知上一個時間戳迭代出來的 後驗狀態 $x_{k|k}$ 和後驗協方差矩陣 $P_{k|k}$ 他們代表當前狀態的分佈。 Sigma點的數量取決於狀態向量的維度 $n_{\sigma}

演算法解析：擴充套件卡爾曼濾波EKF與KF本質分析

       瞭解卡爾曼濾波(KF)的人都應該清楚，擴充套件卡爾曼濾波(EKF)是KF的非線性情況，多做了一步線性化。一般線性化的常用方法使泰勒展開，需要一個展開的初始位置。因此KF的運動方程實際上就是求這個初始位置，但

卡爾曼濾波（Kalman Filter）原理與公式推導

公式推導 領域 公式 不一定 技術 精度 原理 應用 定性 一、背景---卡爾曼濾波的意義 隨著傳感技術、機器人、自動駕駛以及航空航天等技術的不斷發展，對控制系統的精度及穩定性的要求也越來越高。卡爾曼濾波作為一種狀態最優估計的方法，其應用也越來越普遍，如在無人機、機器人等領

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

無人駕駛汽車系統入門（一）——卡爾曼濾波與目標追蹤

前言：隨著深度學習近幾年來的突破性進展，無人駕駛汽車也在這些年開始不斷向商用化推進。很顯然，無人駕駛汽車已經不是遙不可及的“未來技術”了，未來10年

卡爾曼濾波總結——KF、EFK、UKF

1.用途 現實是我們的處理和測量模型都是非線性的，結果就是一個不規則分佈，KF能夠使用的前提就是所處理的狀態是滿足高斯分佈的，為了解決這個問題，EKF是尋找一個線性函式來近似這個非線性函式，而UKF就是去找一個與真實分佈近似的高斯分佈。 KF處理線性模型: EKF 通過雅克比和偏

用俗話講講卡爾曼濾波與粒子濾波

一，卡爾曼濾波 卡爾曼濾波可以根據一些已知的量來預測未知的量，這些量受到的干擾必須得近似高斯噪聲。這個東西可以用來幹什麼呢？例如我們可以用來預測明天，後天，未來好幾天的溫度。我們可以在前幾天用溫度計記

mpu6050姿態解算與卡爾曼濾波（1）數學

定義地理座標系n系：x軸指向東，y軸指向北，z軸指向天。在mpu6050晶片上定義載體座標系b系。那麼b系的姿態就是指n系與b系相對的旋轉關係，即如何由n系旋轉到b系。 描述這種旋轉關係通常使用尤拉角(ψ,θ,γ)T，姿態矩陣T(3x3)，四元數Q=(q0，q

數據會有一些波動和噪點采用卡爾曼濾波算法進行

卡爾曼濾波實現多項式擬合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)#矩陣的型別