ukf(无迹卡尔曼滤波)算法的matlab程序.
ukf(无迹卡尔曼滤波)算法的matlab程序.
% UKF Unscented Kalman Filter for nonlinear dynamic systems
% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
% for nonlinear dynamic system (for simplicity, noises are assumed as additive):
% x_k+1 = f(x_k) + w_k
% z_k = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
% v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs: f: function handle for f(x)
% x: "a priori" state estimate
% P: "a priori" estimated state covariance
% h: fanction handle for h(x)
% z: current measurement
% Q: process noise covariance
% R: measurement noise covariance
% Output: x: "a posteriori" state estimate
% P: "a posteriori" state covariance
%
% Example:
%{
n=3; %number of state
q=0.1; %std of process
r=0.1; %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2; % covariance of measurement
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
h=@(x)x(1); % measurement equation
s=[0;0;1]; % initial state
x=s+q*randn(3,1); %initial state % initial state with noise
P = eye(n); % initial state covraiance
N=20; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(1,N);
for k=1:N
z = h(s) + r*randn; % measurments
sV(:,k)= s; % save actual state
zV(k) = z; % save measurment
[x, P] = ukf(f,x,P,h,z,Q,R); % ekf
xV(:,k) = x; % save estimate
s = f(s) + q*randn(3,1); % update process
end
for k=1:3 % plot results
subplot(3,1,k)
plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
%
% By Yi Cao at Cranfield University, 04/01/2008
%
L=numel(x); %numer of states
m=numel(z); %numer of measurements
alpha=1e-3; %default, tunable
ki=0; %default, tunable
beta=2; %default, tunable
lambda=alpha^2*(L+ki)-L; %scaling factor
c=L+lambda; %scaling factor
Wm=[lambda/c 0.5/c+zeros(1,2*L)]; %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta); %weights for covariance
c=sqrt(c);
X=sigmas(x,P,c); %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of process
% X1=sigmas(x1,P1,c); %sigma points around x1
% X2=X1-x1(:,ones(1,size(X1,2))); %deviation of X1
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurments
P12=X2*diag(Wc)*Z2'; %transformed cross-covariance
K=P12*inv(P2);
x=x1+K*(z-z1); %state update
P=P1-K*P12'; %covariance update
function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
%Input:
% f: nonlinear map
% X: sigma points
% Wm: weights for mean
% Wc: weights for covraiance
% n: numer of outputs of f
% R: additive covariance
%Output:
% y: transformed mean
% Y: transformed smapling points
% P: transformed covariance
% Y1: transformed deviations
L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
for k=1:L
Y(:,k)=f(X(:,k));
y=y+Wm(k)*Y(:,k);
end
Y1=Y-y(:,ones(1,L));
P=Y1*diag(Wc)*Y1'+R;
function X=sigmas(x,P,c)
%Sigma points around reference point
%Inputs:
% x: reference point
% P: covariance
% c: coefficient
%Output:
% X: Sigma points
A = c*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A];
ukf(无迹卡尔曼滤波)算法的matlab程序.相关推荐
- ukf实测信号的预测 matlab,ukf(无迹卡尔曼滤波)算法的matlab程序.
ukf(无迹卡尔曼滤波)算法的matlab程序. function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R) % UKF Unscented Kalman Filt ...
- 基于自适应无迹卡尔曼滤波算法(AUKF)锂电池SOC估计,噪声系数自适应 Matlab程序
基于自适应无迹卡尔曼滤波算法(AUKF)锂电池SOC估计,噪声系数自适应 Matlab程序 仿真模型建模数据mat Sci一篇参考文献 ID:77100657373800632张徳帅123
- 始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别?
原始卡尔曼滤波算法(KF).扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别? 原文:https://www.zhihu.com/question/22714163/answ ...
- UKF无迹卡尔曼滤波
UKF无迹卡尔曼滤波是在卡尔曼滤波和变换的基础上发展而来的,它是利用无损变换使线性假设下的卡尔曼滤波应用于非线性系统. 之前提到的EKF算法简单易操作,在工业中有广泛的应用.但是它也存在很多缺点:需要 ...
- fcm算法的MATLAB实现,FCM算法的matlab程序(初步)
FCM算法的matlab程序 1.采用iris数据库 iris_data.txt 5.1 3.5 1.4 0.2 4.9 3 1.4 0.2 4.7 3.2 1.3 0.2 4.6 3.1 1.5 0 ...
- matlab dfp法,DFP算法及Matlab程序.docx
DFP算法及Matlab程序 作业二 用DFP算法求解,取,.一.求解:求迭代点x1令,得的极小值点,所以得:于是,由DFP修正公式有下一个搜索方向为求迭代点x2令,得的极小值点于是得:,所以:,因H ...
- fdtd算法的matlab程序,FDTD算法的Matlab程序
<FDTD算法的Matlab程序>由会员分享,可在线阅读,更多相关<FDTD算法的Matlab程序(6页珍藏版)>请在人人文库网上搜索. 1.* 5= T$h;O % 3-D ...
- hilbert曲线序编码matlab,Hilbert曲线扫描矩阵的生成算法及其MATLAB程序代码
Hilbert曲线扫描矩阵的生成算法及其MATLAB程序代码 王笋,徐小双(华中科技大学控制科学与工程系,武汉 430074) 摘 要 Hilbert曲线是一种重要的图像处理工具,在图像处理,特别是图 ...
- matlab中emd函数,EMD算法的matlab程序介绍解析
<EMD算法的matlab程序介绍解析>由会员分享,可在线阅读,更多相关<EMD算法的matlab程序介绍解析(6页珍藏版)>请在人人文库网上搜索. 1.此版本为 ALAN 版 ...
最新文章
- 寒门出贵子,传递正能量
- linux centos7 重启服务器报错 Run 'systemctl daemon-reload' to reload units
- 交换两个数不引入第三个变量
- 机房安防系统常见故障原因及处理方法
- 计算机英语基础课程论文,计算机专业英语课程教学论文
- Windows系统下oracle数据库每天定时备份
- 信息学奥赛一本通 1413:确定进制 | OpenJudge NOI 1.13 34:确定进制 | OpenJudge NOI 2.1 1973:确定进制
- 浅谈nodejs中的Crypto模块
- 利用 Webpack 实现小程序多项目管理
- POJ-3264-Balanced Lineup
- Python机器学习库——Sklearn
- 杭电HDUacm2098
- 华硕笔记本电池0%充不进电_华硕笔记本电池0%充不进电
- android js 回调函数,JS回调函数简单易懂的入门实例分析
- PT1000 电桥测温
- 谈谈IT行业的各种证书
- 微信邮箱是什么?微信邮箱怎么注册申请,微信邮箱怎么登陆?
- 由于无法验证发布者 所以windows阻止此软件
- JAVA 删除File文件中的内容。
- 输出全排列(递归非递归)