注册 登录  
 加关注
   显示下一条  |  关闭
温馨提示!由于新浪微博认证机制调整,您的新浪微博帐号绑定已过期,请重新绑定!立即重新绑定新浪微博》  |  关闭

Mr.Right

不顾一切的去想,于是我们有了梦想。脚踏实地的去做,于是梦想成了现实。

 
 
 

日志

 
 
关于我

人生一年又一年,只要每年都有所积累,有所成长,都有那么一次自己认为满意的花开时刻就好。即使一时不顺,也要敞开胸怀。生命的荣枯并不是简单的重复,一时的得失不是成败的尺度。花开不是荣耀,而是一个美丽的结束,花谢也不是耻辱,而是一个低调的开始。

网易考拉推荐

阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter  

2012-06-08 22:35:46|  分类: 编程 |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |
即以此功德,庄严佛净土。上报四重恩,下济三途苦。惟愿见闻者,悉发菩提心。在世富贵全,往生极乐国
% 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 with noise
P = eye(n);                               % initial state covraiance
N=20;                                     % total dynamic steps
 
% allocate memory
xV = zeros(n,N);          %estmate      
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);            % ukf 
  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



function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)
% 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
%
% Reference: Julier, SJ. and Uhlmann, J.K., Unscented Filtering and
% Nonlinear Estimation, Proceedings of the IEEE, Vol. 92, No. 3,
% pp.401-422, 2004. 
%
% 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, 注意L就是下式中的n,表示状态变量的个数
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 c=L+lambda;                                 % 即 n + lambda
Wm=[lambda/c 0.5/c+zeros(1,2*L)];           %weights for means, 即下式中Wm的值
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);               %weights for covariance
 
c=sqrt(c); % 注意原先 c=L+lambda;  此时 c = sqrt(L + lambda)
X=sigmas(x,P,c);                            %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);          %unscented transformation of process
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurments
 P12=X2*diag(Wc)*Z2';                        %transformed cross-covariance
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 
K=P12*inv(P2);
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 
x=x1+K*(z-z1);                              %state update
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 
P=P1-K*P12';                                %covariance update
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 
 

function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
%Input:
%        f: nonlinear map, f 是个函数句柄
%        X: sigma points
%       Wm: weights for mean
%       Wc: weights for covraiance
%        n: number 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));     % X 是Sigma点  
    y=y+Wm(k)*Y(:,k);       
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
  end
 
Y1=Y-y(:,ones(1,L)); 
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 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
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
 
A = c*chol(P)';  %此时 c = sqrt(L + lambda), A就是sqrt(L + lambda) * sqrt(P)
Y = x(:,ones(1,numel(x)));  % 注意Y和x是相同的
X = [x Y+A Y-A]; 
阿英讲无迹卡尔曼滤波器 -- Learning the Unscented Kalman Filter - 阿英 - Mr.Right
  评论这张
 
阅读(1441)| 评论(1)
推荐 转载

历史上的今天

在LOFTER的更多文章

评论

<#--最新日志,群博日志--> <#--推荐日志--> <#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇,下一篇--> <#-- 热度 --> <#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->
 
 
 
 
 
 
 
 
 
 
 
 
 
 

页脚

网易公司版权所有 ©1997-2016