UKF滤波的简单理解与仿真
一、简单理解(不严谨)
UKF的本质是利用随机采样的条件均值代替均值,使用被估计量和量测量的再现样本点计算两者的方差阵。通常通过选取(2n+1)个Sigma点来拟合均值和方差,再用得到的方差去更新卡尔曼增益系数和状态误差。
假设存在非线性系统:
1)首先选取Sigma点(也就是常说的UT变换),使其满足:
2)将得到的Sigma点矩阵乘以相应的状态转移矩阵,得到预测点 Xi(或者表示为Xk|k-1),再次重复步骤1,根据预测值,再次使用UT变换,得到新的sigma点集同理可以得到量测点。通过乘以相应的权值可以得到新的均值协方差。
二、代码仿真
这里转载文章:UKF学习笔记之匀速直线运动目标跟踪_扛着小兵逃跑的博客-CSDN博客的代码
function UKF
clc;
clear;
T=1; % 采样周期
N=60/T; % 采样次数
X=zeros(4,N); % 初始化真实轨迹矩阵
X(:,1)=[100 2 200 20]; % 目标初始位置、速度
Z=zeros(1,N); % 初始化 观测距离矩阵
w =1e-5;
Q = w*diag([0.5,1]); % 过程噪声协方差矩阵
G=[T^2/2, 0;T, 0;0,T^2/2;0, T]; % 过程噪声驱动矩阵
R=5; % 观测噪声协方差
F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; % 状态转移矩阵
Xstation=[200,300]; % 观测位置
V = sqrt(R)*randn(1,N); % v均值为0,协方差为R的高斯白噪声 %sqrt(A)矩阵每个元素分别开方,与矩阵点乘有关;sqrtm(A)矩阵为整体,与矩阵相乘有关
W = sqrt(Q)*randn(2,1); % W均值为0,协方差为Q的高斯白噪声
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N X(:,t)=F*X(:,t-1)+G*W; % 目标的真实轨迹
end
for t=1:NZ(t)=Distance(X(:,t),Xstation)+V(t); % 真实观测目标位置
end
%%%%%%%%%%%%%%%%%%%%%%%%%% UKF滤波 %%%%%%%%%%%%%%%%%%
L=4; % L为状态维度
a=1; % a控制采样点的分布状态
k=1; % k为待选参数 没有界限,但是要保证(n+lamda)*P为半正定矩阵
b=2; % b非负的权系数
lambda=a*a*(L+k)-L; % lambda为缩放比列参数,用于降低总的预测误差
%%%%%%%%%%%%%%%%%%%%%% sigma点的权值 %%%%%%%%%%%%%%%%%
Wm=zeros(1,2*L+1);
Wc=zeros(1,2*L+1);
Wm(1)=lambda/(L+lambda);
Wc(1)=lambda/(L+lambda)+1-a*a+b; % 第一个采样点的均值和协方差的权值
for j=2:2*L+1 % 剩下2n个sigma点的均值和协方差的权值Wm(j)= lambda/(2*(L+lambda)); % 下标m为均值的权值Wc(j)= lambda/(2*(L+lambda)); % 下标c为协方差的权值
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf = zeros(4,N);
Xukf(:,1)=X(:,1); %无迹Kalman滤波状态初始化
P0=eye(4); %协方差阵初始化
for t=2:Nxestimate = Xukf(:,t-1);P=P0;
%%%%%%% 第一步:获得一组采样点,Sigma点集 %%%%%%%%%%%%%%cho=(chol(P*(L+lambda)))'; % cho=chol(X)用于对矩阵X进行Cholesky分解,产生一个上三角阵cho,使cho'cho=X。若X为非对称正定,则输出一个出错信息。for k=1:LxgamaP1(:,k)=xestimate+cho(:,k);xgamaP2(:,k)=xestimate-cho(:,k);endXsigma=[xestimate xgamaP1 xgamaP2]; % 获得 2L+1 个Sigma点
%第二步:对Sigme点集进行一步预测Xsigmapre=F*Xsigma;
%第三步:利用第二部结果计算均值和协方差Xpred=zeros(4,1);for k=1:2*L+1Xpred=Xpred+Wm(k)*Xsigmapre(:,k);endPpred=zeros(4,4); %协力差阵预测for k=1:2*L+1Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';endPpred=Ppred+G*Q*G';
%第四步:根据预测值,再次使用UT变换,得到新的sigma点集chor=(chol((L+lambda)*Ppred))';for k=1:LXaugsigmaP1(:,k)=Xpred+chor(:,k);XaugsigmaP2(:,k)=Xpred-chor(:,k);endXaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
%第五步:观测预测for k=1:2*L+1 %观测预测Zsigmapre(1,k)=h(Xaugsigma(:,k),Xstation);end
%第六步:计算观测预测均值和协方差Zpred=0; %观测预测的均值for k=1:2*L+1Zpred=Zpred+ Wm(k)*Zsigmapre(1,k);endPzz=0;for k=1:2*L+1Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';endPzz=Pzz+R; %得到协方差PzzPxz= zeros(4,1);for k=1:2*L+1Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';end
%第七步:计算Kalman增益K=Pxz/Pzz; % Kalman
%第八步:状态和方差更新Xukf(:,t)=Xpred+K*(Z(t)-Zpred);% 状态更新P=Ppred-K*Pzz*K'; %方差更新P0=P;
end
%6误差分析
Err_KalmanFilter=zeros(2,N);
for i=1:NErr_KalmanFilter(1,i)=X(1,i)-Xukf(1,i); %滤波后的误差Err_KalmanFilter(2,i)=X(3,i)-Xukf(3,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
hold on;box on;
plot(X(1,:),X(3,:),'-B.'); %真实轨迹
plot(Xukf(1,:),Xukf(3,:),'-rs'); %无迹Kalman滤波轨迹
legend('真实轨迹','UKF轨迹')
xlabel('横向坐标-X/m')
ylabel('纵向坐标-Y');
figure
hold on;
box on;
plot(Err_KalmanFilter(1,:),'-kd','MarkerFace','r');
xlabel('时间/s');
ylabel('横向坐标误差/m');
figure
plot(Err_KalmanFilter(2,:),'-bd','MarkerFace','g');
xlabel('时间/s');
ylabel('纵向坐标误差/m');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%子函数:求两点间的距离
function d=Distance(X1,X2)d=sqrt((X1(1)- X2(1))^2 + (X1(3)-X2(2))^2);
end
%观测子函数:观测距离
function [y]=h(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
end
UKF滤波的简单理解与仿真相关推荐
- 高通滤波与低通滤波的简单理解
设 x[n]为采样数据 y[n]为滤波结果 初始y[0]=x[0]; 高通滤波; i从1到n-1循环操作 y[i] = α * y[i-1] + α * (x[i] - x[i-1]) a<1, ...
- 无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞 机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
- Deep Reinforcement Learning: Pong from Pixels翻译和简单理解
原文链接: http://karpathy.github.io/2016/05/31/rl/ 文章目录 原文链接: 前言 Policy-Gradient结构流程图 Deep Reinforcement ...
- 直立车模控制中三种滤波算法简单分析(清华卓晴)
摘自:https://mp.weixin.qq.com/s/WbCh0NFAnsf9y2blQenf7g 让我想起余义的一篇文章也是说到平衡车有三种滤波,我想和卓晴说的是一样的吧. https://b ...
- 简单理解EMC(电磁兼容)
简单理解EMC(电磁兼容) 1.EMC三要素 电磁骚扰源 敏感设备的抗扰度 骚扰的传输途径 2.EMC的三大基本控制技术 屏蔽技术 接地技术 滤波技术 通俗来讲三板斧就是屏蔽接地套磁环 3.产品的EM ...
- Android:安卓学习笔记之Bitmap的简单理解和使用
Android Bitmap的简单理解和使用 Android Bitmap 一.Bitmap的定义 二.Bitmap的格式 2.1 存储格式 2.2 压缩格式 三.Bitmap创建方法 3.1 Bit ...
- 简单理解VIO(二)
文章目录 一.旋转运动学 二.IMU测量模型及运动模型 1.MEMS加速度计工作原理 2.陀螺仪测量原理 三.IMU误差模型 1.六面法标定加速度 1.1.bias与scale 1.2.轴间误差 四. ...
- android 点击事件消费,Android View事件分发和消费源码简单理解
Android View事件分发和消费源码简单理解 前言: 开发过程中觉得View事件这块是特别烧脑的,看了好久,才自认为看明白.中间上网查了下singwhatiwanna粉丝的读书笔记,有种茅塞顿开 ...
- 【转载】Deep learning:十九(RBM简单理解)
Deep learning:十九(RBM简单理解) 这篇博客主要用来简单介绍下RBM网络,因为deep learning中的一个重要网络结构DBN就可以由RBM网络叠加而成,所以对RBM的理解有利于我 ...
最新文章
- 文档计算机无法分页,同一EXCEL文件在不同计算机上显示分页不同解决办法(6页)-原创力文档...
- 如何删除未推送的git commit?
- NOIP提高组复赛 知识点整理
- 宣化市大专计算机学校,2018张家口专科大学有哪些 最新大专院校名单
- Python机器学习:SVM006什么是核函数?
- python爬取网页时返回http状态码HTTP Error 418以及如何查看自己的User-Agent
- 32bit win7 在VMWARE中安装64位的redhat LINUX4.7
- layui可以动态添加div吗_乳化剂是什么?可以添加到护肤品里吗?
- 使用Thrift让Python为Java提供服务
- matlab 运算子图_PHP运算子
- python集成包地址 Anaconda 一键安装拥有所有包
- project2016资源管理
- Linux命令面试突击
- 苹果6s最大屏幕尺寸_苹果 iPhone 12 Pro DXOMARK 屏幕评分 87 分,最大问题是黄色色偏 - 苹果,iPhone...
- 禁止页面在浏览器中打开 只能在微信内核浏览器中打开
- 洲际酒店集团大中华区开业酒店突破600家;因美纳中国生产制造基地正式启用 | 美通企业日报...
- 天水訟 (易經大意 韓長庚)
- 超详细 PHP 开发环境配置:WampServer+ZendStudio+XDebug
- 千里之行,始于足下--致2013-2014上半年总结
- 还有谁!!!?谁是Uber下一个要颠覆的行业?
热门文章
- 实现NRF52832蓝牙DFU无线升级
- 基于 Vue+Spring 前后端分离管理系统ELAdmin
- Pycharm 许可证过期解决
- 如何生成一个二维码?
- One-Way Streets (oneway)
- windows 网络远程连接samba,并修改windows默认连接samba端口445到指定端口(支持监听ipv6 及ipv4 IP地址)
- 【项目实战】Python基于OpenCV和卷积神经网络CNN进行车牌号码识别项目实战
- Unity游戏开发客户端面经——C#(初级)
- word2007表格计算机,电脑员好做吗?使用word2007表格?
- 计算机技术调剂控制工程,控制工程294求调剂 - 考研 - 小木虫 - 学术 科研 互动社区...