无迹卡尔曼滤波算法(UKF)详细推倒及其仿真(matlab)
著名学者Julier等提出近似非线性函数的均值和方差远比近似非线性函数本身更容易,因此提出了基于确定性采样的UKF算法。
该算法的核心思想是:采用UT变换,利用一组Sigma采样点来描述随机变量的高斯分布,然后通过非线性函数的传递,再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。
相比于EKF,UKF的估计精度能够达到泰勒级数展开的二阶精度。
- UT变换
- 采样策略
- UKF算法流程
- UKF算法仿真
采样策略:
根据Sigma点采样策略不同,相应的Sigma点以及均值权值和方差权值也不尽相同,因此UT变换的估计精度也会有差异,但总体来说,其估计精度能够达到泰勒级数展开的二阶精度。
为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性,所以采样点的选取应满足:
下面介绍两种经常使用的采样策略:比例采样和比例修正对称采样
至此UKF算法介绍完毕。
UKF算法仿真:
仿真的例子与上一篇EKF仿真的非线性系统相同。
主程序:
clear all; clc; tf = 50; Q = 10;w=sqrt(Q)*randn(1,tf); R = 1;v=sqrt(R)*randn(1,tf); P =eye(1); x=zeros(1,tf); Xnew=zeros(1,tf); x(1,1)=0.1; Xnew(1,1)=x(1,1); z=zeros(1,tf); z(1)=x(1,1)^2/20+v(1); zjian=zeros(1,tf); zjian(1,1)=z(1); linear = 0.5; for k = 2 : tf % 模拟系统 x(:,k) = linear * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1)^2)) + 8 * cos(1.2*(k-1)) + w(k-1); %状态值 z(k) = (x(:,k)^2 / 20) + v(k);%观测值 f=@(x)(linear * x + (2.5 * x / (1 + x^2)) + 8 * cos(1.2*(k-1))); h=@(x)(x^2 / 20); [Xnew(:,k),P(:,:,k)] = ukf(f,Xnew(:,k-1),P(:,:,k-1),h,z(k),Q,R); end figure; t = 2 : tf; plot(t,x(1,t),'b',t,Xnew(1,t),'r:'); legend('真实值','UKF估计值');
Sigma点集选取(2n+1个):
function X = sigmas(x,P,c) % x:参考点,P:协方差,c:系数,X:Sigma点 A = c*chol(P)'; Y = x(:,ones(1,numel(x))); X = [x Y+A Y-A];
UT变换:
function [y,Y,P,Y1] = ut(f,X,Wm,Wc,n,R) %f:非线性函数,X: sigma点 Wm:均值权值 Wc:方差权值 L = size(X,2); y = zeros(n,1); Y = zeros(n,L); for k=1:L Y(:,k) = f(X(:,k)); %非线性传递后结果 r y = y+Wm(k)*Y(:,k); %均值 end Y1 = Y-y(:,ones(1,L)); P = Y1*diag(Wc)*Y1'+R; %协方差
UKF:
function [x,P] = ukf(fstate, x, P, hmeas, z, Q, R) L = numel(x); %状态数量 m = numel(z); %量测数量 a = 1e-3; %默认 ki = 0; %默认 beta = 2; %默认 lambda = a^2*(L+ki)-L; c = L+lambda; Wm = [lambda/c 0.5/c+zeros(1,2*L)]; Wc = Wm; Wc(1) = Wc(1)+(1-a^2+beta); c = sqrt(c); X = sigmas(x,P,c); [x1,X1,P1,X2] = ut(fstate,X,Wm,Wc,L,Q); [z1,Z1,P2,Z2] = ut(hmeas,X1,Wm,Wc,m,R); % 滤波部分 P12 = X2*diag(Wc)*Z2'; K = P12*inv(P2); x = x1+K*(z-z1); P = P1-K*P12';
仿真结果:
- 点赞
- 收藏
- 分享
- 文章举报
- 从贝叶斯滤波理论到容积卡尔曼滤波算法(CKF)详细推导及编程实现常转弯率模型估计。(matlab)
- 基于距离的粒子滤波跟踪系统及其matlab仿真
- ukf(无迹卡尔曼滤波)算法的matlab程序.
- UKF 程序matlab仿真
- Matlab仿真PID控制(带详细代码编写和参数分析)
- 基于纯方位粒子滤波目标跟踪及其matlab仿真
- 基于多站纯方位粒子滤波跟踪系统及其 4000 matlab仿真
- MATLAB各类函数详细讲解 simulike系统仿真分析
- 神经网络及其matlab仿真
- Matlab--基于前馈补偿的PID控制算法及其仿真
- 粒子滤波原理及其matlab仿真
- 非高斯模型粒子滤波跟踪系统及其matlab仿真
- 粒子滤波原理及其matlab仿真
- C语言main函数的参数及其返回值详细解析
- 极化码的matlab仿真(2)——编码
- 直线检测算法 LSD 的相关介绍及其在 OpenCV 和 MATLAB 中的可视化
- OFDM的MATLAB仿真
- 详细介绍DES算法,RSA算法及其特点.
- Matlab Robotics Toolbox 仿真计算:Kinematics, Dynamics, Trajectory Generation
- Asp.net 2.0 自定义控件开发专题[详细探讨页面状态(视图状态和控件状态)机制及其使用场景](示例代码下载)