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

易拉罐的博客

心静自然凉

 
 
 

日志

 
 

转 Kalman滤波器 仿真  

2010-03-25 15:08:25|  分类: matlab仿真 |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |
1.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% KALMANF - updates a system state vector estimate based upon an
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% observation, using a discrete Kalman filter.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% Version 1.0, June 302004
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% This tutorial function was written by Michael C. Kleder
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (Comments are appreciated at: public@kleder.com)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% INTRODUCTION
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% Many people have heard of Kalman filtering, but regard the topic
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% as mysterious. While it's true that deriving the Kalman filter and
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% proving mathematically that it is "optimal" under a variety of
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% circumstances can be rather intense, applying the filter to
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% a basic linear system is actually very easy. This Matlab file is
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% intended to demonstrate that.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% An excellent paper on Kalman filtering at the introductory level,
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% without detailing the mathematical underpinnings, is:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% "An Introduction to the Kalman Filter"
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% Greg Welch and Gary Bishop, University of North Carolina
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% PURPOSE:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% The purpose of each iteration of a Kalman filter is to update
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% the estimate of the state vector of a system (and the covariance
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% of that vector) based upon the information in a new observation.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% The version of the Kalman filter in this function assumes that
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% observations occur at fixed discrete time intervals. Also, this
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% function assumes a linear system, meaning that the time evolution
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% of the state vector can be calculated by means of a state transition
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% matrix.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% USAGE:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s = kalmanf(s)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% "s" is a "system" struct containing various fields used as input
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% and output. The state estimate "x" and its covariance "P" are
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% updated by the function. The other fields describe the mechanics
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% of the system and are left unchanged. A calling routine may change
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% these other fields as needed if state dynamics are time-dependent;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% otherwise, they should be left alone after initial values are set.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% The exceptions are the observation vectro "z" and the input control
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (or forcing function) "u." If there is an input function, then
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% "u" should be set to some nonzero value by the calling routine.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% SYSTEM DYNAMICS:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% The system evolves according to the following difference equations,
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% where quantities are further defined below:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% x = Ax + Bu + w meaning the state vector x evolves during one time
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% step by premultiplying by the "state transition
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% matrix" A. There is optionally (if nonzero) an input
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% vector u which affects the state linearly, and this
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% linear effect on the state is represented by
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% premultiplying by the "input matrix" B. There is also
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% gaussian process noise w.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% z = Hx + v meaning the observation vector z is a linear function
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% of the state vector, and this linear relationship is
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% represented by premultiplication by "observation
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% matrix" H. There is also gaussian measurement
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% noise v.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% v ~ N(0,R) meaning v is gaussian noise with covariance R
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% VECTOR VARIABLES:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.x = state vector estimate. In the input structthis is the
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% "a priori" state estimate (prior to the addition of the
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% information from the new observation). In the output struct,
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% this is the "a posteriori" state estimate (after the new
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% measurement information is included).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.z = observation vector
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.u = input control vector, optional (defaults to zero).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% MATRIX VARIABLES:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.A = state transition matrix (defaults to identity).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.P = covariance of the state vector estimate. In the input struct,
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% this is "a priori," and in the output it is "a posteriori."
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (required unless autoinitializing as described below).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.B = input matrix, optional (defaults to zero).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.Q = process noise covariance (defaults to zero).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.R = measurement noise covariance (required).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% s.H = observation matrix (defaults to identity).
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% NORMAL OPERATION:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (1) define all state definition fields: A,B,H,Q,R
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (2) define intial state estimate: x,P
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (3) obtain observation and control vectors: z,u
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (4) call the filter to obtain updated state estimate: x,P
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% (5return to step (3) and repeat
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% INITIALIZATION:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% If an initial state estimate is unavailable, it can be obtained
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% from the first observation as follows, provided that there are the
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% same number of observable variables as state variables. This "auto-
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% intitialization" is done automatically if s.x is absent or NaN.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%= inv(H)*z
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%= inv(H)*R*inv(H')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% This is mathematically equivalent to setting the initial state estimate
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% covariance to infinity.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% SCALAR EXAMPLE (Automobile Voltimeter):
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% % Define the system as a constant of 12 volts:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客function T
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客clear s
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.x 
= 12;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.A 
= 1;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% % Define a process noise (stdev) of 2 volts as the car operates:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.Q 
= 2^2% variance, hence stdev^2
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% Define the voltimeter to measure the voltage itself:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.H 
= 1;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% % Define a measurement error (stdev) of 2 volts:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.R 
= 2^2% variance, hence stdev^2
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
%Do not define any system input (control) functions:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.B 
= 0;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.u 
= 0;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% % Do not specify an initial state:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.x 
= nan;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客s.P 
= nan;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% % Generate random voltages and watch the filter operate.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客tru
=[]; % truth voltage
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
for t=1:20
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    tru(end
+1= randn*2+12;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s(end).z 
= tru(end) + randn*2% create a measurement
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s(end
+1)=kalmanf(s(end)); % perform a Kalman filter iteration
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% figure
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% hold on
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% grid on
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% % plot measurement data:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    hz
=plot([s(1:end-1).z],'r');hold on
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% % plot a-posteriori state estimates:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    hk
=plot([s(2:end).x],'b-');hold on
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    ht
=plot(tru,'g-');hold on
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    legend(
'observations','Kalman output','true voltage',0)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    title(
'Automobile Voltimeter Example')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% hold off
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客end    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客function s 
= kalmanf(s)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
% set defaults for absent fields:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'x'); s.x=nan*z; end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'P'); s.P=nan; end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'z'); error('Observation vector missing'); end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'u'); s.u=0; end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'A'); s.A=eye(length(x)); end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'B'); s.B=0; end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'Q'); s.Q=zeros(length(x)); end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'R'); error('Observation covariance missing'); end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if ~isfield(s,'H'); s.H=eye(length(x)); end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
if isnan(s.x)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% initialize state estimate from first observation
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
if diff(size(s.H))
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客        error(
'Observation matrix must be square and invertible for state autointialization.');
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s.x 
= inv(s.H)*s.z;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s.P 
= inv(s.H)*s.R*inv(s.H'); 
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
else
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% This is the code which implements the discrete Kalman filter:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% Prediction for state vector and covariance:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s.x 
= s.A*s.x + s.B*s.u;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s.P 
= s.A * s.P * s.A' + s.Q;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% Compute Kalman gain factor:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    K 
= s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% Correction based on observation:
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s.x 
= s.x + K*(s.z-s.H*s.x);
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    s.P 
= s.P - K*s.H*s.P;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% Note that the desired result, which is an improved estimate
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% of the sytem state vector x and its covariance P, was obtained
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% in only five lines of code, once the system was defined. (That's
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
    % how simple the discrete Kalman filter is to use.) Later,
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
% we'll discuss how to deal with nonlinear systems.
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客

 

2.


 

转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% 状态
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% xk=A?xk-1+B?uk+wk
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% zk=H?xk+vk,
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% p(w) ~ N(0,Q)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% p(v) ~ N(0,R),
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% 预测
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% x'k=A?xk+B?uk
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% P'k=A?P(k-1)*AT + Q
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% 修正
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% Kk=P'k?HT?(H?P'k?HT+R)-1
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% xk=x'k+Kk?(zk-H?x'k)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客% Pk=(I-Kk?H)?P'k
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客%要注意的是:必须把系统状态和kalman滤波器内部预测的状态分开
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客function Test
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客A=[1 0.1;0 1];
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客B=0;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Xp=rand(2,1)*0.1; 
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客X=[0 0]';
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客H=[1 0];
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Q=eye(2)*1e-5;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客R=eye(1)*0.1; 
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客P=eye(2);% P'(k)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客angle=[];
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客angle_m=[];
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客angle_real=[];
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客for i=1:500
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    angle_real=[angle_real X(1)]; %实际角度
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    [Xp,P]=Predict(A,Xp,P,Q); 
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    X=A*X+rand(2,1)*1e-5;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    z_m=H*X+rand(1,1)*0.1-0.05;  
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    angle_m=[angle_m z_m(1)];   %测量的角度
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客        
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    [Xp,P]=Correct(P,H,R,X,z_m);
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客    angle=[angle Xp(1)];     %预测的角度    
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客end
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客t=1:500;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客plot(t,angle,'r',t,angle_m,'g',t,angle_real,'b')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客legend('预测值','测量值','实际值')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客figure
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客plot(t,angle-angle_real,'r',t,angle_m-angle_real,'g')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客legend('滤波后的误差','测量的误差')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客title('误差分析')
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客xlabel('time');
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客ylabel('error');
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客function [Xk,Pk]=Predict(A,Xk,Pk_1,Q)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Xk=A*Xk;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Pk=A*Pk_1*A'+Q;
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客function [Xk,Pk]=Correct(Pk,H,R,Xk,zk)
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Kk=Pk * H' * inv(H * Pk * H' + R);
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Xk=Xk+ Kk*(zk-H*Xk);
转 Kalman滤波器 仿真 - 易拉罐bb - 易拉罐的博客Pk=(eye(size(Pk,1)) - Kk*H)*Pk;

 

 

 

  评论这张
 
阅读(313)| 评论(0)
推荐 转载

历史上的今天

在LOFTER的更多文章

评论

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

页脚

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