前言：上一篇文章的最後我們提到卡爾曼濾波存在著一個非常大的侷限性——它僅能對線性的處理模型和測量模型進行精確的估計，在非線性的場景中並不能達到最優的估計效果。所以之前為了保證我們的處理模型是線性的，我們上一節中使用了恆定速度模型，然後將估計目標的加減速用處理噪聲來表示，這一模型用來估算行人的狀態其實已經足夠了，但是在現實的駕駛環境中，我們不僅要估計行人，我們除了估計行人狀態以外，我們還需要估計其他車輛，自行車等等狀態，他們的狀態估計顯然不能使用簡單的線性系統來描述，這裡，我們介紹非線性系統情況下的一種廣泛使用的濾波演算法——擴充套件卡爾曼濾波(Extended Kalman Filter, EKF)

。

本節主要講解非線性系統中廣泛使用的擴充套件卡爾曼濾波演算法，我們通常將該演算法應用於實際的車輛狀態估計（或者說車輛追蹤）中。另外，實際的車輛追蹤運動模型顯然不能使用簡單的恆定速度模型來建模，在本節中會介紹幾種應用於車輛追蹤的高階運動模型。並且已經其中的CTRV模型來構造我們的擴充套件卡爾曼濾波。最後，在程式碼例項中，我會介紹如何使用EKF做多感測器融合。

應用於車輛追蹤的高階運動模型

首先要明確的一點是，不管是什麼運動模型，本質上都是為了幫助我們簡化問題，所以我們可以根據運動模型的複雜程度（次數）來給我們常用的運動模型分一下類。 
一次運動模型（也別稱為線性運動模型）：

• 恆定速度模型（Constant Velocity, CV）
• 恆定加速度模型（Constant Acceleration, CA）

這些線性運動模型假定目標是直線運動的，並不考慮物體的轉彎。

二次運動模型：

• 恆定轉率和速度模型（Constant Turn Rate and Velocity，CTRV）
• 恆定轉率和加速度模型（Constant Turn Rate and Acceleration，CTRA）

CTRV目前多用於機載追蹤系統（飛機），這些二次運動模型大多假定速度 vv 和 偏航角速度（yaw rate） ωω 沒有關係，因此，在這類運動模型中，由於偏航角速度測量的擾動（不穩定），即使車輛沒有移動，我們的運動模型下的角速度也會發生細微的變化。

為了解決這個問題，速度 v

v 和 偏航角速度 ωω 的關聯可以通過設定轉向角 ΦΦ 恆定來建立，這樣就引出了 恆定轉向角和速度模型（Constant Steering Angle and Velocity，CSAV）, 另外，速度可以別假定為線性變化的，進而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。

這些運動模型的關係如圖：

運動模型的狀態轉移公式

由於除CCA以外，以上的運動模型都非常著名，故本文不提供詳細的推導過程。本文提供CV和CTRV模型的狀態轉移公式。

狀態轉移公式：就是我們的處理模型由上一狀態的估計計算下一個狀態的先驗分佈的計算公式，可以理解為我們基於一定的先驗知識總結出來的運動公式。

1. CV模型： 
   CV模型的狀態空間可以表示為： 
   

   x→(t)=(xyvxvy)Tx→(t)=(xyvxvy)T
   那麼轉移函式為： 
   x→(t+Δt)=⎛⎝⎜⎜⎜⎜x(t)+Δtvxy(t)+Δtvyvxvy⎞⎠⎟⎟⎟⎟x→(t+Δt)=(x(t)+Δtvxy(t)+Δtvyvxvy)

2. CTRV模型： 
   在CTRV中，目標的狀態量為： 
   

   x→(t)=(xyvθω)Tx→(t)=(xyvθω)T
   其中，θθ 為偏航角，是追蹤的目標車輛在當前車輛座標系下與x軸的夾角，逆時針方向為正，取值範圍是[00, 2π2π), ωω是偏航角速度。CTRV的狀態轉移函式為： 
   x→(t+Δt)=⎛⎝⎜⎜⎜⎜⎜⎜vωsin(ωΔt+θ)−vωsin(θ)+x(t)−vωcos(ωΔt+θ)+vωcos(θ)+y(t)vωΔt+θω⎞⎠⎟⎟⎟⎟⎟⎟x→(t+Δt)=(vωsin⁡(ωΔt+θ)−vωsin⁡(θ)+x(t)−vωcos⁡(ωΔt+θ)+vωcos⁡(θ)+y(t)vωΔt+θω)

本文下面的內容將以CTRV模型作為我們的運動模型。使用CTRV還存在一個問題，那就是 ω=0ω=0 的情況，此時我們的狀態轉移函式公式中的 (x,y)(x,y) 將變成無窮大。為了解決這個問題，我們考察一下ω=0ω=0 的情況，此時我們追蹤的車輛實際上是直線行駛的，所以我們的 (x,y)(x,y) 的計算公式就變成了： 

x(t+Δt)=vcos(θ)Δt+x(t)x(t+Δt)=vcos⁡(θ)Δt+x(t)y(t+Δt)

相關推薦

無人駕駛汽車系統入門（二）——高階運動模型和擴充套件卡爾曼濾波

前言：上一篇文章的最後我們提到卡爾曼濾波存在著一個非常大的侷限性——它僅能對線性的處理模型和測量模型進行精確的估計，在非線性的場景中並不能達到最優的估計效果。所以之前為了保證我們的處理模型是線性的，我們上一節中使用了恆定速度模型，然後將估計目標的加減速用處理噪聲來表示，這一模

無人駕駛汽車系統入門（二十一）——基於Frenet優化軌跡的無人車動作規劃方法

動作規劃動作在無人車規劃模組的最底層，它負責根據當前配置和目標配置生成一序列的動作，我們前面討論的三次樣條插值實際上只是一個簡單的路徑，而非我們最終能夠執行的軌跡，本文介紹一種基於Frenet座標系的優化軌跡動作規劃方法，該方法在高速情況下的高階車道保持和

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

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

“無人駕駛汽車系統入門”部落格專欄

“無人駕駛汽車系統入門”專欄連結地址： https://blog.csdn.net/column/details/28410.html?&page=1 前言：隨著深度學習近幾年來的突破性進展，無人駕駛汽車也在這些年開始不斷向商用化推進。很顯然，無人駕駛汽車已經不是遙不

MyBatis入門（二）—— 輸入映射和輸出映射、動態sql、關聯查詢

輸出類型 sql name屬性 一對一 test HA h標簽 自動 CI p.p4 { margin: 0.0px 0.0px 0.0px 10.0px; font: 10.5px "PingFang SC" } p.p6 { margin: 0.0px 0.0px 0.

02 React快速入門（二）——this的指向和資料修改問題

問題描述：       在react學習中，通過定義一個按鈕，然後為此按鈕繫結一個事件，此事件主要的功能就是使用者點選按鈕，就會在相應的頁面增加一個item選項。在程式碼層面來說，在程式碼裡的state下的list會增加一個數據，如下所示：  

無人駕駛四 擴充套件卡爾曼濾波在目標車輛運動狀態識別中的運用（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

Maven入門（二）專案的打包和執行

在上一篇（建立第一個Maven專案）介紹了Maven的編譯和測試， 編譯和測試通過後就是打包和運行了。 一、打包 命令：mvn clean package 執行命令後輸出資訊如下： 打包後在target/目錄中會生成helloworld-1.0-SNAPSHOT.jar

初學者的卡爾曼濾波——擴充套件卡爾曼濾波（一）

簡介 　　已經歷經了半個世紀的卡爾曼濾波至今仍然是研究的熱點，相關的文章不斷被髮表。其中許多文章是關於卡爾曼濾波器的新應用，但也不乏改善和擴充套件濾波器演算法的研究。而對演算法的研究多著重於將卡爾曼濾波應用於非線性系統。 　　為什麼學界要這麼熱衷於將卡爾曼濾波器用於非線性系統呢？因為卡爾曼濾波器從一開

慣性導航——擴充套件卡爾曼濾波（一）

對於無人機的慣性導航系統，系統的狀態方程是非線性的，根據擴充套件卡爾曼濾波方程： Predict x^k|k−1Pk|k−1=f(x^k−1|k−1,uk−1)=Fk−1Pk−1|k−1FT

擴充套件卡爾曼濾波（EKF）

首先進行文件下載 仔細閱讀文件，理解文件中所述內容。 我對文件的matlab程式碼進行了簡單調整如下： %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Android入門之文件系統操作（二）文件操作相關指令

-h tools strong abc his art 為什麽 重命名 path （一）獲取總根 [java] view plain copy File[] fileList=File.listRoots(); //返回fileList.length為1 /

區塊鏈快速入門（二）——分布式系統核心技術

分布式系 rand 2pc 串行化 可擴展性 分布式數據庫 ner web 偏序 區塊鏈快速入門（二）——分布式系統核心技術 一、分布式系統的一致性問題 1、分布式系統的一致性問題 隨著摩爾定律碰到瓶頸，越來越多情況下要依靠可擴展的分布式架構來實現海量處理能力。單點結構演變

【領卓教育】Linux學習入門（二）------系統命令之 打補丁

      打補丁，什麼意思，在玩LOL、王者、吃雞...等遊戲的時候，可能大家都遇到過打打補丁的過程，例如版本的升級就是一次打補丁的過程！       首先版本的升級和更新，小編最初以為是把遊戲重新安裝或者覆蓋安裝的過程，可是每次的升

樹莓派入門（二） - 樹莓派系統備份指令碼，Raspberry pi

系統備份步驟 簡述 全卡備份 建立img檔案 使用軟體備份 壓縮備份 原理 備份流程 安裝工具 儲存映象到U盤 建立img檔案 對img檔案分割槽 格式化img檔

入門 | 無人駕駛汽車系統基本框架梳理

作者簡介：申澤邦（Adam Shan），蘭州大學在讀碩士研究生，主攻無人駕駛，深度學習；蘭大未來

（數字IC）低功耗設計入門（二）——功耗的分析

layout 變化 監視 merge obj source divide 傳播 總結 　　前面學習了進行低功耗的目的個功耗的構成，今天就來分享一下功耗的分析。由於是面向數字IC前端設計的學習，所以這裏的功耗分析是基於DC中的power compiler工具；更精確的功耗分析

Linux系統管理（二）（網絡服務）

網絡服務 linux redhat 紅帽 點擊下載：Linux系統管理（二）（網絡服務）本文出自 “飛奔的魚兒” 博客，請務必保留此出處http://feiyuer.blog.51cto.com/6967044/1931978Linux系統管理（二）（網絡服務）

ext2文件系統學習（二）—— 目錄磁盤結構

echo free 文件格式 htm file 目錄結構 bitmap 點號 name 創建鏡像、mount等操作和上一篇一樣，測試目錄結構如下： 一些文件系統信息如下： Block size: 1024 Inodes per group: 1

Linux入門（二）

man linux終端 linux發行版本 linux文件系統初步 google高級用法 Linux常用的基礎命令1.發行版本2.CISC、RISC3.編譯和反編譯（GPL、LGPL、BSD）4.程序包管理5.文件系統初步終端設備虛擬終端圖形終端串行終端偽終端Linux的哲學思想6.開源協