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

Mr.Right

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

 
 
 

日志

 
 
关于我

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

网易考拉推荐

阿英讲扩展卡尔曼滤波器--Learning the Extended Kalman Filter  

2012-06-08 21:04:23|  分类: 编程 |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |

即以此功德,庄严佛净土。上报四重恩,下济三途苦。惟愿见闻者,悉发菩提心。在世富贵全,往生极乐国。

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] = ekf(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

 

 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 注意EKF除了用雅可比矩阵作为状态矩阵的量测矩阵外和KF一模一样


[x1,A]=jaccsd(fstate,x);    % 从第n-1时刻状态的后验估计x得到第n时刻状态x的先验估计x1

阿英讲扩展卡尔曼滤波器--Learning the Extended Kalman Filter - 阿英 - Mr.Right

 P=A*P*A'+Q;                 %  第n-1时刻状态x的后验估计的误差协方差递推第n时刻状态x的先验估计的误差协方差,时间更新

阿英讲扩展卡尔曼滤波器--Learning the Extended Kalman Filter - 阿英 - Mr.Right

 [z1,H]=jaccsd(hmeas,x1);    % z1是无噪声时对状态输出z的估计


P12=P*H';                   %cross covariance
% K=P12*inv(H*P12+R);       %Kalman filter gain

阿英讲扩展卡尔曼滤波器--Learning the Extended Kalman Filter - 阿英 - Mr.Right

 % x=x1+K*(z-z1);            % 从第n-1时刻状态的先验估计x1得到第n时刻状态的后验估计x

阿英讲扩展卡尔曼滤波器--Learning the Extended Kalman Filter - 阿英 - Mr.Right

 % P=P-K*P12';               % 用第n时刻状态x的先验估计的误差协方差计算第n时刻状态后验估计的后验估计误差协方差

阿英讲扩展卡尔曼滤波器--Learning the Extended Kalman Filter - 阿英 - Mr.Right

 R=chol(H*P12+R);            %Cholesky factorization,我以前没想到是以上的等价,快速算法
U=P12/R;                    %K=U/R'; Faster because of back substitution
x=x1+U*(R'\(z-z1));         %Back substitution to get state update
P=P-U*U';                   %Covariance update, U*U'=P12/R/R'*P12'=K*P12.

function [z,A]=jaccsd(fun,x) % 我以前不知道Jabobian矩阵的数值算法竟然是这样干的
% JACCSD Jacobian through complex step differentiation
% [z J] = jaccsd(f,x)
% z = f(x)
% J = f'(x)
% Example
% f=@(x)[x(1)*x(2)^2;x(1)*x(3)^2;x(1)^2];
% x=1:3;
% [J,z]=jaccsd(f,x)
% J =
%     4     4     0
%     9     0     6
%     2     0     0
% z =
%     4
%     9
%     1
%
z=fun(x);
n=numel(x);
m=numel(z);
A=zeros(m,n);
h=n*eps;
for k=1:n
    x1=x;
    x1(k)=x1(k)+h*i;
    A(:,k)=imag(fun(x1))/h; %这样算Jacobian矩阵确实很妙
end

  评论这张
 
阅读(1166)| 评论(1)
推荐 转载

历史上的今天

在LOFTER的更多文章

评论

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

页脚

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