簡(jiǎn)介本資源是一份面向信號(hào)處理、導(dǎo)航定位及非線性濾波方向研究者與高年級(jí)本科生的高斯粒子濾波GM-PFMatlab實(shí)現(xiàn)代碼聚焦解決非線性、非高斯系統(tǒng)下的狀態(tài)估計(jì)難題適用于目標(biāo)跟蹤、機(jī)器人定位、傳感器融合等典型場(chǎng)景。壓縮包僅含1個(gè)核心文件——Particle_GS.m為純腳本型Matlab源碼無GUI、無依賴庫體積僅1KB結(jié)構(gòu)緊湊完整涵蓋粒子初始化、非線性預(yù)測(cè)、基于高斯混合權(quán)重的重采樣、觀測(cè)更新與后驗(yàn)狀態(tài)加權(quán)估計(jì)等關(guān)鍵環(huán)節(jié)代碼注釋清晰便于逐行理解算法邏輯與數(shù)學(xué)實(shí)現(xiàn)細(xì)節(jié)。目前已有170人學(xué)習(xí)下載是掌握高斯混合粒子濾波原理、對(duì)比標(biāo)準(zhǔn)PF性能退化問題、調(diào)試粒子數(shù)量與高斯分量設(shè)置影響的理想輕量級(jí)實(shí)踐素材。1. 項(xiàng)目概述從“黑箱”到“透明”的追蹤利器如果你在信號(hào)處理、機(jī)器人定位或者金融時(shí)間序列分析等領(lǐng)域摸爬滾打過一定對(duì)“狀態(tài)估計(jì)”這個(gè)核心問題不陌生。簡(jiǎn)單來說就是如何從一堆充滿噪聲的觀測(cè)數(shù)據(jù)里把系統(tǒng)內(nèi)部那些你看不見、摸不著的真實(shí)狀態(tài)給“猜”出來。這就像你只聽到隔壁房間傳來的模糊腳步聲和物品碰撞聲卻要準(zhǔn)確判斷出里面的人在什么位置、正在做什么動(dòng)作一樣。傳統(tǒng)的卡爾曼濾波Kalman Filter是解決線性高斯系統(tǒng)的一把好手但現(xiàn)實(shí)世界遠(yuǎn)比這復(fù)雜——系統(tǒng)可能是非線性的噪聲也可能不服從高斯分布。這時(shí)粒子濾波Particle Filter就登場(chǎng)了它用一群“粒子”來模擬概率分布理論上可以處理任何非線性、非高斯問題堪稱“萬能鑰匙”。然而這把“萬能鑰匙”有個(gè)眾所周知的痛點(diǎn)粒子退化。隨著濾波迭代絕大多數(shù)粒子的權(quán)重會(huì)變得微乎其微只有少數(shù)幾個(gè)粒子“撐場(chǎng)面”導(dǎo)致計(jì)算資源浪費(fèi)估計(jì)精度急劇下降。重采樣技術(shù)能緩解但又會(huì)帶來粒子多樣性喪失的“樣本枯竭”問題。于是各路大神開始琢磨能不能讓這些粒子“生”得更好一點(diǎn)Particle_GS.zip這個(gè)項(xiàng)目或者說“高斯粒子濾波”Gaussian Particle Filter的核心思路正是對(duì)這一痛點(diǎn)的精準(zhǔn)回應(yīng)。它不像傳統(tǒng)粒子濾波那樣讓粒子在狀態(tài)空間里“隨機(jī)游走”而是試圖用更聰明、更高效的方式——特別是引入高斯過程Gaussian Process或類似思想——來指導(dǎo)粒子的提議分布讓每一次預(yù)測(cè)都更有方向每一次更新都更有效率。這不僅僅是另一個(gè)濾波算法它代表了一種思路的升級(jí)從“暴力模擬”走向“智能引導(dǎo)”。2. 核心思路拆解為何是“高斯”與“粒子”的聯(lián)姻要理解高斯粒子濾波的精髓我們得先看看傳統(tǒng)粒子濾波的“笨辦法”。在標(biāo)準(zhǔn)的序貫重要性采樣SIS粒子濾波中我們通常直接從系統(tǒng)的狀態(tài)轉(zhuǎn)移方程即先驗(yàn)分布中抽取粒子。比如在機(jī)器人定位中你根據(jù)上一時(shí)刻的位置和運(yùn)動(dòng)模型可能包含噪聲直接預(yù)測(cè)下一時(shí)刻粒子可能在哪。這個(gè)方法簡(jiǎn)單直接但問題在于它完全忽略了當(dāng)前時(shí)刻的最新觀測(cè)數(shù)據(jù)。這就好比蒙著眼睛扔飛鏢純靠上一秒的手感而不看靶子在哪。2.1 最優(yōu)提議分布的理想與現(xiàn)實(shí)理論上存在一個(gè)“最優(yōu)提議分布”它既考慮了上一時(shí)刻的狀態(tài)先驗(yàn)又融入了當(dāng)前時(shí)刻的觀測(cè)似然。從這個(gè)分布里采樣產(chǎn)生的粒子權(quán)重方差最小效率最高。但這個(gè)最優(yōu)分布往往復(fù)雜到無法直接采樣。高斯粒子濾波的“高斯”二字正是對(duì)這個(gè)難題的一種逼近策略。其核心思想是用高斯分布來近似這個(gè)最優(yōu)提議分布。為什么是高斯分布因?yàn)楦咚狗植季哂辛己玫臄?shù)學(xué)性質(zhì)共軛性、易于采樣、參數(shù)少并且根據(jù)中心極限定理許多自然過程在局部可以近似為高斯的。具體來說高斯粒子濾波的步驟可以概括為高斯近似在得到當(dāng)前時(shí)刻的觀測(cè)數(shù)據(jù)后算法會(huì)嘗試為每一個(gè)粒子或一類粒子計(jì)算一個(gè)高斯形式的提議分布。這個(gè)分布的均值會(huì)朝著能更好解釋當(dāng)前觀測(cè)的方向偏移其協(xié)方差則反映了這種估計(jì)的不確定性。從高斯分布中采樣新的粒子不再是從粗糙的先驗(yàn)分布中抽取而是從這個(gè)精心構(gòu)造的、融合了觀測(cè)信息的高斯提議分布中抽取。權(quán)重計(jì)算修正由于采樣分布改變了粒子權(quán)重的計(jì)算公式也需要相應(yīng)調(diào)整以保持估計(jì)的無偏性。這樣一來新生代的粒子天生就“更靠近”真實(shí)狀態(tài)的可能性區(qū)域極大地減少了無效粒子緩解了退化問題。2.2 Particle_GS的可能技術(shù)內(nèi)涵項(xiàng)目名中的“GS”很可能指向Gaussian Sum或Gaussian-Sampling相關(guān)的技術(shù)。這暗示了其實(shí)現(xiàn)可能不止一種簡(jiǎn)單的高斯近似高斯和粒子濾波Gaussian Sum Particle Filter, GSPF這是最直接的聯(lián)想。它不滿足于用一個(gè)高斯分布去近似復(fù)雜的后驗(yàn)分布而是用多個(gè)高斯分布的加權(quán)和即高斯混合模型來近似。每個(gè)高斯分量可以捕獲后驗(yàn)分布的不同模式比如多目標(biāo)跟蹤中多個(gè)可能的目標(biāo)位置。采樣時(shí)先按權(quán)重選擇一個(gè)高斯分量再?gòu)脑摲至恐胁蓸印＿@大大提升了對(duì)于多峰分布的表達(dá)能力?；诟咚惯^程回歸的引導(dǎo)另一種更現(xiàn)代的思路是利用高斯過程Gaussian Process, GP這種非參數(shù)模型來學(xué)習(xí)狀態(tài)轉(zhuǎn)移或觀測(cè)模型中的未知非線性函數(shù)并利用GP預(yù)測(cè)的均值和方差來構(gòu)造一個(gè)時(shí)變、自適應(yīng)的高斯提議分布。這對(duì)于模型不確定性強(qiáng)的問題特別有效。無論具體是哪種技術(shù)路徑其目標(biāo)都是一致的通過引入“高斯”這一結(jié)構(gòu)化的概率工具為“粒子”的演化提供更科學(xué)的導(dǎo)航從而在保持粒子濾波通用性的同時(shí)大幅提升其采樣效率和估計(jì)精度。3. 算法實(shí)現(xiàn)與關(guān)鍵步驟詳解讓我們拋開復(fù)雜的數(shù)學(xué)推導(dǎo)從一個(gè)實(shí)踐者的角度看看如何構(gòu)建一個(gè)基礎(chǔ)版本的高斯粒子濾波。這里我們以實(shí)現(xiàn)一個(gè)“擴(kuò)展卡爾曼濾波輔助的粒子濾波”為例它是最常見也最直觀的一種高斯粒子濾波實(shí)現(xiàn)方式有時(shí)被稱為EKPFExtended Kalman Particle Filter或高斯提議粒子濾波。3.1 系統(tǒng)模型定義假設(shè)我們有一個(gè)標(biāo)準(zhǔn)的非線性離散時(shí)間系統(tǒng)狀態(tài)方程x_k f(x_{k-1}, u_{k-1}) w_{k-1}。x是狀態(tài)f是非線性狀態(tài)轉(zhuǎn)移函數(shù)u是控制輸入w是過程噪聲通常假設(shè)為高斯白噪聲。觀測(cè)方程z_k h(x_k) v_k。z是觀測(cè)值h是非線性觀測(cè)函數(shù)v是觀測(cè)噪聲通常假設(shè)為高斯白噪聲。我們的目標(biāo)是在每個(gè)時(shí)刻k根據(jù)從1到k的所有觀測(cè)z_{1:k}估計(jì)出狀態(tài)x_k的后驗(yàn)概率分布p(x_k | z_{1:k})。3.2 算法核心循環(huán)步驟假設(shè)在k-1時(shí)刻我們擁有N個(gè)帶權(quán)粒子集合{x_{k-1}^(i), w_{k-1}^(i)}其中i1,...,N且權(quán)重已歸一化。對(duì)于每一個(gè)粒子i執(zhí)行以下步驟3.2.1 為每個(gè)粒子運(yùn)行一次EKF更新這是高斯粒子濾波區(qū)別于標(biāo)準(zhǔn)粒子濾波的關(guān)鍵。我們不是直接用f函數(shù)傳播粒子而是為每個(gè)粒子x_{k-1}^(i)維護(hù)一個(gè)局部的均值和協(xié)方差(m_{k-1}^(i), P_{k-1}^(i))。通常初始化時(shí)m_{k-1}^(i) x_{k-1}^(i)P_{k-1}^(i)為一個(gè)較小的初始協(xié)方差矩陣。預(yù)測(cè)步EKF Predict計(jì)算雅可比矩陣F_{k-1}^(i) ?f/?x |_{xm_{k-1}^(i)}。預(yù)測(cè)均值m_{k|k-1}^(i) f(m_{k-1}^(i), u_{k-1})。預(yù)測(cè)協(xié)方差P_{k|k-1}^(i) F_{k-1}^(i) P_{k-1}^(i) (F_{k-1}^(i))^T Q。其中Q是過程噪聲w的協(xié)方差矩陣。更新步EKF Update計(jì)算觀測(cè)雅可比矩陣H_k^(i) ?h/?x |_{xm_{k|k-1}^(i)}。計(jì)算觀測(cè)殘差新息y_k^(i) z_k - h(m_{k|k-1}^(i))。計(jì)算新息協(xié)方差S_k^(i) H_k^(i) P_{k|k-1}^(i) (H_k^(i))^T R。其中R是觀測(cè)噪聲v的協(xié)方差矩陣。計(jì)算卡爾曼增益K_k^(i) P_{k|k-1}^(i) (H_k^(i))^T (S_k^(i))^{-1}。更新均值m_k^(i) m_{k|k-1}^(i) K_k^(i) y_k^(i)。更新協(xié)方差P_k^(i) (I - K_k^(i) H_k^(i)) P_{k|k-1}^(i)。至此我們?yōu)榈趇個(gè)粒子得到了一個(gè)高斯提議分布N(m_k^(i), P_k^(i))。這個(gè)分布融合了該粒子的歷史信息和當(dāng)前最新觀測(cè)是“個(gè)性化定制”的最優(yōu)分布近似。3.2.2 從高斯提議分布中采樣從我們剛計(jì)算出的分布中抽取新粒子x_k^(i) ~ N(m_k^(i), P_k^(i))注意這里有一個(gè)非常重要的細(xì)節(jié)。m_k^(i)是EKF更新后的狀態(tài)估計(jì)它已經(jīng)包含了當(dāng)前觀測(cè)z_k的信息。因此從這個(gè)分布采樣得到的x_k^(i)天生就比從先驗(yàn)f(x_{k-1}^(i), ...)采樣得到的粒子更可能處于高似然區(qū)域。3.2.3 計(jì)算并更新粒子權(quán)重權(quán)重的計(jì)算必須考慮我們改變了采樣分布從先驗(yàn)分布改為了高斯提議分布。權(quán)重更新公式為w_k^(i) ∝ w_{k-1}^(i) * [ p(z_k | x_k^(i)) * p(x_k^(i) | x_{k-1}^(i)) ] / [ q(x_k^(i) | x_{k-1}^(i), z_k) ]其中p(z_k | x_k^(i))是似然即觀測(cè)模型。對(duì)于高斯觀測(cè)噪聲p(z_k | x_k^(i)) N(z_k; h(x_k^(i)), R)。p(x_k^(i) | x_{k-1}^(i))是先驗(yàn)概率即狀態(tài)轉(zhuǎn)移模型。對(duì)于高斯過程噪聲p(x_k^(i) | x_{k-1}^(i)) N(x_k^(i); f(x_{k-1}^(i), u_{k-1}), Q)。q(x_k^(i) | x_{k-1}^(i), z_k)就是我們使用的提議分布即N(x_k^(i); m_k^(i), P_k^(i))。實(shí)操心得在實(shí)際編程中為了避免數(shù)值下溢我們通常計(jì)算對(duì)數(shù)權(quán)重并在所有粒子計(jì)算完成后通過減去最大值再指數(shù)化的方式進(jìn)行歸一化。計(jì)算高斯分布的概率密度時(shí)也使用其對(duì)數(shù)形式。3.2.4 重采樣計(jì)算有效粒子數(shù)N_eff 1 / (sum_{i1}^N (w_k^(i))^2)。如果N_eff低于設(shè)定的閾值如N/2則進(jìn)行重采樣如系統(tǒng)重采樣、殘差重采樣等。重采樣后粒子集合變?yōu)閧x_k^(i), 1/N}即所有權(quán)重置為均等。循環(huán)結(jié)束對(duì)所有N個(gè)粒子完成上述步驟后我們就得到了k時(shí)刻新的粒子集{x_k^(i), w_k^(i)}。狀態(tài)的估計(jì)可以取加權(quán)平均E[x_k] ≈ sum_{i1}^N w_k^(i) * x_k^(i)。4. 代碼實(shí)現(xiàn)要點(diǎn)與避坑指南理論很美好但代碼實(shí)現(xiàn)時(shí)處處是坑。下面結(jié)合Python偽代碼分享幾個(gè)關(guān)鍵實(shí)現(xiàn)點(diǎn)和常見陷阱。4.1 核心數(shù)據(jù)結(jié)構(gòu)設(shè)計(jì)import numpy as np from scipy.stats import multivariate_normal class GaussianParticleFilter: def __init__(self, N, dim_x, f, h, Q, R): self.N N # 粒子數(shù) self.dim_x dim_x # 狀態(tài)維度 self.f f # 狀態(tài)轉(zhuǎn)移函數(shù) f(x, u) self.h h # 觀測(cè)函數(shù) h(x) self.Q Q # 過程噪聲協(xié)方差 self.R R # 觀測(cè)噪聲協(xié)方差 # 初始化粒子每個(gè)粒子除了狀態(tài)還需要維護(hù)其局部均值和協(xié)方差 self.particles np.random.randn(N, dim_x) # 狀態(tài) self.weights np.ones(N) / N self.means self.particles.copy() # 每個(gè)粒子的局部均值 self.covariances np.array([np.eye(dim_x) * 0.1 for _ in range(N)]) # 每個(gè)粒子的局部協(xié)方差4.2 EKF更新步驟的實(shí)現(xiàn)細(xì)節(jié)def _ekf_update_for_particle(self, particle_idx, u, z): 為單個(gè)粒子執(zhí)行EKF預(yù)測(cè)與更新返回其高斯提議分布的參數(shù) (mean, cov) m_prev self.means[particle_idx] P_prev self.covariances[particle_idx] # 1. 預(yù)測(cè)步 # 計(jì)算雅可比矩陣 F。對(duì)于簡(jiǎn)單模型可以解析求導(dǎo)復(fù)雜模型建議使用自動(dòng)微分或數(shù)值差分。 F self._compute_jacobian_f(m_prev, u) # 需要實(shí)現(xiàn)此函數(shù) m_pred self.f(m_prev, u) P_pred F P_prev F.T self.Q # 2. 更新步 H self._compute_jacobian_h(m_pred) # 需要實(shí)現(xiàn)此函數(shù) z_pred self.h(m_pred) y z - z_pred # 新息 S H P_pred H.T self.R K P_pred H.T np.linalg.inv(S) # 卡爾曼增益 m_updated m_pred K y P_updated (np.eye(self.dim_x) - K H) P_pred return m_updated, P_updated重要提示雅可比矩陣的計(jì)算是精度和穩(wěn)定性的關(guān)鍵。對(duì)于復(fù)雜模型使用autograd、JAX或PyTorch的自動(dòng)微分功能是更穩(wěn)健的選擇。如果使用數(shù)值差分如有限差分法務(wù)必小心選擇步長(zhǎng)過大會(huì)導(dǎo)致誤差大過小會(huì)受浮點(diǎn)數(shù)精度影響。4.3 權(quán)重計(jì)算的數(shù)值穩(wěn)定性這是最容易出問題的地方。直接計(jì)算高斯概率密度值尤其是高維情況下極易導(dǎo)致下溢結(jié)果為0。def _compute_log_weight(self, particle_state, m_updated, P_updated, x_prev, u, z): 計(jì)算對(duì)數(shù)權(quán)重 # 提議分布的對(duì)數(shù)概率密度 log_q multivariate_normal.logpdf(particle_state, meanm_updated, covP_updated) # 先驗(yàn)分布的對(duì)數(shù)概率密度 prior_mean self.f(x_prev, u) log_prior multivariate_normal.logpdf(particle_state, meanprior_mean, covself.Q) # 似然函數(shù)的對(duì)數(shù)概率密度 likelihood_mean self.h(particle_state) log_likelihood multivariate_normal.logpdf(z, meanlikelihood_mean, covself.R) # 權(quán)重公式的對(duì)數(shù)形式: log(w) log(prior) log(likelihood) - log(q) log_w log_prior log_likelihood - log_q return log_w def update_weights(self, log_weights): 將對(duì)數(shù)權(quán)重轉(zhuǎn)換為歸一化的線性權(quán)重保證數(shù)值穩(wěn)定 # 減去最大值防止指數(shù)運(yùn)算溢出 max_log_w np.max(log_weights) scaled_log_w log_weights - max_log_w # 指數(shù)化并歸一化 weights np.exp(scaled_log_w) weights / np.sum(weights) return weights4.4 重采樣的選擇與實(shí)現(xiàn)重采樣會(huì)引入額外的計(jì)算開銷和樣本多樣性問題。系統(tǒng)重采樣Systematic Resampling在大多數(shù)情況下是效果和效率的較好平衡。def systematic_resample(self): 系統(tǒng)重采樣 indices np.zeros(self.N, dtypeint) # 生成一個(gè)[0, 1/N)區(qū)間內(nèi)的隨機(jī)起點(diǎn) step 1.0 / self.N u np.random.rand() * step # 計(jì)算累積權(quán)重 cumulative_sum np.cumsum(self.weights) i 0 for j in range(self.N): while cumulative_sum[i] u: i 1 indices[j] i u step # 根據(jù)索引復(fù)制粒子、均值、協(xié)方差并重置權(quán)重 self.particles self.particles[indices] self.means self.means[indices] self.covariances self.covariances[indices] self.weights np.ones(self.N) / self.N避坑指南協(xié)方差正定性在EKF更新中理論上P_updated應(yīng)是半正定的但數(shù)值計(jì)算可能導(dǎo)致其失去正定性。一個(gè)補(bǔ)救措施是使用(P_updated P_updated.T) / 2來強(qiáng)制對(duì)稱或采用更穩(wěn)定的平方根濾波實(shí)現(xiàn)如UKF。粒子數(shù)選擇高斯粒子濾波雖然效率高但仍需足夠粒子來覆蓋不確定性。維度災(zāi)難依然存在。一個(gè)經(jīng)驗(yàn)法則是狀態(tài)維度d粒子數(shù)N至少應(yīng)在10d到100d之間具體取決于問題的非線性程度和噪聲大小。重采樣頻率不要每個(gè)時(shí)間步都重采樣。頻繁重采樣會(huì)迅速耗盡粒子多樣性。通常根據(jù)有效粒子數(shù)N_eff動(dòng)態(tài)決定是否重采樣。5. 應(yīng)用場(chǎng)景與性能分析高斯粒子濾波并非銀彈它在特定場(chǎng)景下優(yōu)勢(shì)明顯在另一些場(chǎng)景下可能優(yōu)勢(shì)不大甚至增加不必要的復(fù)雜度。5.1 優(yōu)勢(shì)場(chǎng)景強(qiáng)非線性、弱非高斯的單峰估計(jì)問題這是其主戰(zhàn)場(chǎng)。例如機(jī)器人SLAM同時(shí)定位與建圖機(jī)器人在已知地圖中的定位運(yùn)動(dòng)模型和觀測(cè)模型如激光雷達(dá)非線性很強(qiáng)但真實(shí)位置通常只有一個(gè)單峰。EKF輔助的粒子濾波能顯著減少所需粒子數(shù)實(shí)現(xiàn)實(shí)時(shí)定位。目標(biāo)跟蹤單目標(biāo)在雷達(dá)或視頻中跟蹤一個(gè)機(jī)動(dòng)目標(biāo)觀測(cè)距離、角度與狀態(tài)位置、速度關(guān)系非線性。高斯粒子濾波比標(biāo)準(zhǔn)粒子濾波收斂更快跟蹤更穩(wěn)。金融時(shí)間序列波動(dòng)率估計(jì)例如用隨機(jī)波動(dòng)率模型SV模型估計(jì)資產(chǎn)價(jià)格的隱含波動(dòng)率狀態(tài)空間高度非線性高斯提議能有效捕捉波動(dòng)率的變化路徑。計(jì)算資源受限但對(duì)實(shí)時(shí)性要求高因?yàn)榱Ｗ有矢呖梢杂酶俚牧Ｗ舆_(dá)到與標(biāo)準(zhǔn)粒子濾波相近的精度從而節(jié)省計(jì)算時(shí)間適合嵌入式系統(tǒng)或高頻交易場(chǎng)景。5.2 劣勢(shì)或需謹(jǐn)慎使用的場(chǎng)景多峰后驗(yàn)分布問題如果真實(shí)后驗(yàn)分布有多個(gè)相距甚遠(yuǎn)的峰值如數(shù)據(jù)關(guān)聯(lián)模糊的多目標(biāo)跟蹤、某些故障診斷問題單一的高斯提議分布可能會(huì)“忽視”掉某些模式導(dǎo)致粒子全部聚集到其中一個(gè)峰上估計(jì)發(fā)生偏誤。這時(shí)高斯和粒子濾波GSPF或多假設(shè)跟蹤是更好的選擇。極度非高斯噪聲如果過程噪聲或觀測(cè)噪聲明顯偏離高斯分布如重尾分布、均勻分布強(qiáng)行用高斯分布去近似最優(yōu)提議分布可能效果不佳甚至不如簡(jiǎn)單的先驗(yàn)提議。狀態(tài)維度極高在高維空間中構(gòu)造和采樣高維高斯分布的計(jì)算成本尤其是協(xié)方差矩陣的求逆和分解會(huì)變得非常高昂可能抵消其帶來的采樣效率優(yōu)勢(shì)。此時(shí)需要考慮降維或使用更稀疏的結(jié)構(gòu)。5.3 與其它先進(jìn)算法的對(duì)比為了更清晰地定位高斯粒子濾波我們將其與幾個(gè)常見變種進(jìn)行對(duì)比算法核心思想優(yōu)點(diǎn)缺點(diǎn)適用場(chǎng)景標(biāo)準(zhǔn)粒子濾波 (SIR)從先驗(yàn)分布采樣根據(jù)似然重加權(quán)。實(shí)現(xiàn)簡(jiǎn)單理論通用性強(qiáng)。粒子退化嚴(yán)重需要大量粒子。算法驗(yàn)證、教學(xué)、對(duì)精度要求不高的快速原型。無跡粒子濾波 (UPF)用無跡變換UT代替EKF為每個(gè)粒子生成Sigma點(diǎn)計(jì)算高斯提議參數(shù)。無需計(jì)算雅可比矩陣對(duì)強(qiáng)非線性系統(tǒng)近似精度通常高于EKF。計(jì)算量比EKF-PF略大需計(jì)算多個(gè)Sigma點(diǎn)。狀態(tài)轉(zhuǎn)移或觀測(cè)模型非線性很強(qiáng)且求導(dǎo)困難或不可導(dǎo)的場(chǎng)景。高斯粒子濾波 (GPF/EKPF)用EKF為每個(gè)粒子計(jì)算高斯提議分布。有效利用當(dāng)前觀測(cè)大幅減輕退化中等計(jì)算量。依賴局部線性化雅可比對(duì)極度非線性可能失效協(xié)方差可能不正定。大多數(shù)非線性單峰估計(jì)問題的首選實(shí)用方案如機(jī)器人定位、單目標(biāo)跟蹤。高斯和粒子濾波 (GSPF)用高斯混合模型多個(gè)高斯分量的和近似后驗(yàn)。能處理多峰分布表達(dá)能力更強(qiáng)。計(jì)算復(fù)雜需要管理多個(gè)分量及其權(quán)重的合并與剪枝。多模式估計(jì)問題如多目標(biāo)跟蹤初始階段、存在多個(gè)可能故障源的診斷。正則化粒子濾波 (RPF)重采樣后對(duì)離散粒子進(jìn)行連續(xù)化“平滑”處理從核密度估計(jì)中采樣新粒子。能增加粒子多樣性緩解樣本枯竭。引入額外的平滑參數(shù)帶寬需要選擇。與其它粒子濾波結(jié)合使用作為改善重采樣后多樣性的后處理步驟。6. 調(diào)參與實(shí)戰(zhàn)經(jīng)驗(yàn)分享紙上得來終覺淺絕知此事要躬行。最后分享一些在真實(shí)項(xiàng)目中調(diào)試和應(yīng)用高斯粒子濾波的血淚經(jīng)驗(yàn)。6.1 參數(shù)調(diào)優(yōu)的“手感”過程噪聲Q與觀測(cè)噪聲R這是濾波器的“調(diào)音旋鈕”。Q調(diào)大表示你更信任觀測(cè)濾波器反應(yīng)更快但可能更震蕩Q調(diào)小表示你更信任模型濾波結(jié)果更平滑但可能滯后。R反映了你對(duì)觀測(cè)數(shù)據(jù)的信任程度。一個(gè)實(shí)用的調(diào)試方法在系統(tǒng)平穩(wěn)運(yùn)行時(shí)記錄一段時(shí)間的觀測(cè)新息預(yù)測(cè)觀測(cè)與實(shí)際觀測(cè)之差計(jì)算其樣本協(xié)方差這個(gè)值應(yīng)該與濾波器內(nèi)部計(jì)算的S矩陣新息協(xié)方差大致匹配。如果不匹配就需要調(diào)整Q和R。初始協(xié)方差P0粒子初始散布的范圍。設(shè)得太小可能無法覆蓋真實(shí)初始狀態(tài)設(shè)得太大收斂會(huì)變慢。如果不確定可以設(shè)得稍大一些濾波器通常能在幾次迭代后自行收斂。重采樣閾值通常設(shè)為N_eff N/2或N_eff 0.7N。閾值設(shè)得太高會(huì)導(dǎo)致頻繁重采樣多樣性喪失快設(shè)得太低則退化嚴(yán)重?？梢员O(jiān)控N_eff隨時(shí)間的變化曲線來輔助設(shè)定。6.2 診斷濾波器是否“健康”看權(quán)重分布在重采樣前查看粒子權(quán)重的分布。理想情況是權(quán)重分布相對(duì)均勻沒有個(gè)別粒子權(quán)重獨(dú)占鰲頭0.9。如果出現(xiàn)這種情況說明提議分布可能不夠好或者模型/噪聲假設(shè)有誤?？从行ЯＷ訑?shù)N_eff它應(yīng)在一個(gè)合理的范圍內(nèi)波動(dòng)而不是持續(xù)快速下降。如果N_eff持續(xù)走低即使重采樣也無濟(jì)于事這是濾波器發(fā)散Divergence的明確信號(hào)?？葱孪⑿蛄欣碚撋蠘?biāo)準(zhǔn)化新息新息除以S的平方根應(yīng)服從標(biāo)準(zhǔn)正態(tài)分布。你可以繪制其自相關(guān)圖如果存在顯著的自相關(guān)說明濾波器未充分利用所有信息或者模型有誤。6.3 當(dāng)濾波器發(fā)散時(shí)怎么辦檢查模型這是首要原因。你的f(x,u)和h(x)是否準(zhǔn)確描述了物理過程雅可比矩陣計(jì)算是否正確這是最根本的。檢查噪聲統(tǒng)計(jì)量Q和R是否設(shè)置合理嘗試適當(dāng)增大Q給模型更多不確定性。增加粒子數(shù)這是最粗暴但往往有效的方法。嘗試不同的提議分布如果EKF線性化誤差太大可以嘗試切換到UPF無跡粒子濾波它對(duì)于非線性程度的魯棒性更好。引入正則化或馬氏重采樣在重采樣步驟中加入正則化或使用基于馬氏距離的重采樣有助于保持粒子多樣性。高斯粒子濾波是一座連接經(jīng)典卡爾曼濾波與蒙特卡洛方法的堅(jiān)實(shí)橋梁。它用“高斯”的智慧馴服了“粒子”的野性。理解其原理掌握其實(shí)現(xiàn)細(xì)節(jié)并積累足夠的調(diào)試經(jīng)驗(yàn)?zāi)憔湍茉趶?fù)雜的非線性估計(jì)問題中獲得一件強(qiáng)大而趁手的工具。它可能不是最炫酷的算法但絕對(duì)是工程師工具箱里經(jīng)久耐用、值得信賴的那一個(gè)。本文還有配套的精品資源點(diǎn)擊獲取