关于估计隐藏状态和无迹卡尔曼滤波（Matlab 实现）

“在代码的海洋里，有无尽的知识等待你去发现。我就是那艘领航的船，带你乘风破浪，驶向代码的彼岸。

 💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

隐藏状态通常不能直接观测到，但对于理解和控制系统的行为却起着关键作用。例如，在目标跟踪、机器人导航、金融市场预测等众多领域，准确估计隐藏状态可以为决策提供重要依据。 无迹卡尔曼滤波（Unscented Kalman Filter，UKF）是一种用于估计隐藏状态的强大算法。它是卡尔曼滤波的一种扩展，旨在处理非线性系统。传统的卡尔曼滤波在处理线性系统时非常有效，但对于非线性系统，其性能可能会受到限制。 无迹卡尔曼滤波通过一种称为“无迹变换”的方法来近似非线性系统的状态分布。它选择一组称为“sigma 点”的样本点，这些点经过非线性系统的传播后，可以用来估计系统的状态均值和协方差。与传统的线性化方法相比，无迹变换能够更准确地捕捉非线性系统的特性，从而提高状态估计的精度。 无迹卡尔曼滤波具有以下优点：首先，它能够处理高度非线性的系统，而无需对系统进行线性化近似。其次，它在计算效率上相对较高，适合实时应用。此外，它还可以处理非高斯噪声和不确定性，具有较强的鲁棒性。 总之，估计隐藏状态对于许多实际应用至关重要，而无迹卡尔曼滤波作为一种有效的状态估计方法，在非线性系统中具有广泛的应用前景。它为解决复杂动态系统的状态估计问题提供了一种强大的工具。

📚2 运行结果

主函数部分代码：

function FitzHughNagumo(~,~,~)

orient tall

% Dimensions: dq for param. vector, dx augmented state, dy observation
dq=1; dx=dq+2; dy=1;

ll=800; % number of data samples
dT=0.2; % sampling time step (global variable)

% Simulating data: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

x0=zeros(2,ll); x0(:,1)=[0; 0]; % true trajectory
dt=.1*dT; nn=fix(dT/dt); % the integration time step is smaller than dT

% External input, estimated as parameter p later on:
z=(1:ll)/250*2*pi; z=-.4-1.01*abs(sin(z/2));

% 4th order Runge-Kutta integrator for FitzHugh-Nagumo system with external input:
for n=1:ll-1
    xx=x0(:,n);
    for i=1:nn
        k1=dt*FitzHughNagumo_int(xx,z(n));
        k2=dt*FitzHughNagumo_int(xx+k1/2,z(n));
        k3=dt*FitzHughNagumo_int(xx+k2/2,z(n));
        k4=dt*FitzHughNagumo_int(xx+k3,z(n));
        xx=xx+k1/6+k2/3+k3/3+k4/6;
    end
    x0(:,n+1)=xx;
end

x=[z; x0]; % augmented state vector (notation a bit different to paper)

R=.2^2*var(FitzHughNagumo_obsfct(x))*eye(dy,dy); % observation noise covariance matrix
rng('default'); rng(0)
y=FitzHughNagumo_obsfct(x)+sqrtm(R)*randn(dy,ll); % noisy data

FitzHughNagumo_fct(dq,x,dT); % this is just to suppress an erroneous warning and otherwise has no function

% Initial conditions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

xhat=zeros(dx,ll);
xhat(:,2)=x(:,2); % first guess of x_1 set to observation

Q=.015; % process noise covariance matrix

Pxx=zeros(dx,dx,ll);
Pxx(:,:,1)=blkdiag(Q,R,R);

errors=zeros(dx,ll);
Ks=zeros(dx,dy,ll);  % Kalman gains

% Main loop for recursive estimation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for k=2:ll
    
    [xhat(:,k),Pxx(:,:,k),Ks(:,:,k)]=ut(xhat(:,k-1),Pxx(:,:,k-1),y(:,k),'FitzHughNagumo_fct','FitzHughNagumo_obsfct',dq,dx,dy,R,dT);
    
    Pxx(1,1,k)=Q;
    
    errors(:,k)=sqrt(diag(Pxx(:,:,k)));
    
end % k

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]左云龙,张晓锋,朱天高,等.基于滤波平滑的迭代无迹卡尔曼滤波算法[J/OL].海军工程大学学报:1-7[2024-09-02].http://kns.cnki.net/kcms/detail/42.1106.E.20240829.1319.002.html.

[2]邓浩楠,赵治国,赵坤,等.四驱车辆交互式多模型自适应无迹卡尔曼滤波路面附着系数估计[J].汽车工程,2024,46(08):1357-1369.DOI:10.19562/j.chinasae.qcgc.2024.08.003.

🌈4 Matlab代码实现