首页 | 新闻 | 新品 | 文库 | 方案 | 视频 | 下载 | 商城 | 开发板 | 数据中心 | 座谈新版 | 培训 | 工具 | 博客 | 论坛 | 百科 | GEC | 活动 | 主题月 | 电子展
返回列表 回复 发帖

Kalman滤波器实现(3)

Kalman滤波器实现(3)

本帖最后由 look_w 于 2017-9-23 15:40 编辑

Matlab程序看过来下面的一段Matlab代码是从网上找到的,程序简单直接,但作为学习分析用很棒,
% KALMANF - updates a system state vector estimate based upon an%           observation, using a discrete Kalman filter.%% Version 1.0, June 30, 2004%% This tutorial function was written by Michael C. Kleder%% INTRODUCTION%% Many people have heard of Kalman filtering, but regard the topic% as mysterious. While it's true that deriving the Kalman filter and% proving mathematically that it is "optimal" under a variety of% circumstances can be rather intense, applying the filter to% a basic linear system is actually very easy. This Matlab file is% intended to demonstrate that.%% An excellent paper on Kalman filtering at the introductory level,% without detailing the mathematical underpinnings, is:% "An Introduction to the Kalman Filter"% Greg Welch and Gary Bishop, University of North Carolina% PURPOSE:%% The purpose of each iteration of a Kalman filter is to update% the estimate of the state vector of a system (and the covariance% of that vector) based upon the information in a new observation.% The version of the Kalman filter in this function assumes that% observations occur at fixed discrete time intervals. Also, this% function assumes a linear system, meaning that the time evolution% of the state vector can be calculated by means of a state transition% matrix.%% USAGE:%% s = kalmanf(s)%% "s" is a "system" struct containing various fields used as input% and output. The state estimate "x" and its covariance "P" are% updated by the function. The other fields describe the mechanics% of the system and are left unchanged. A calling routine may change% these other fields as needed if state dynamics are time-dependent;% otherwise, they should be left alone after initial values are set.% The exceptions are the observation vectro "z" and the input control% (or forcing function) "u." If there is an input function, then% "u" should be set to some nonzero value by the calling routine.%% SYSTEM DYNAMICS:%% The system evolves according to the following difference equations,% where quantities are further defined below:%% x = Ax + Bu + w  meaning the state vector x evolves during one time%                  step by premultiplying by the "state transition%                  matrix" A. There is optionally (if nonzero) an input%                  vector u which affects the state linearly, and this%                  linear effect on the state is represented by%                  premultiplying by the "input matrix" B. There is also%                  gaussian process noise w.% z = Hx + v       meaning the observation vector z is a linear function%                  of the state vector, and this linear relationship is%                  represented by premultiplication by "observation%                  matrix" H. There is also gaussian measurement%                  noise v.% 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%% VECTOR VARIABLES:%% s.x = state vector estimate. In the input struct, this is the%       "a priori" state estimate (prior to the addition of the%       information from the new observation). In the output struct,%       this is the "a posteriori" state estimate (after the new%       measurement information is included).% s.z = observation vector% s.u = input control vector, optional (defaults to zero).%% MATRIX VARIABLES:%% s.A = state transition matrix (defaults to identity).% s.P = covariance of the state vector estimate. In the input struct,%       this is "a priori," and in the output it is "a posteriori."%       (required unless autoinitializing as described below).% s.B = input matrix, optional (defaults to zero).% s.Q = process noise covariance (defaults to zero).% s.R = measurement noise covariance (required).% s.H = observation matrix (defaults to identity).%% NORMAL OPERATION:%% (1) define all state definition fields: A,B,H,Q,R% (2) define intial state estimate: x,P% (3) obtain observation and control vectors: z,u% (4) call the filter to obtain updated state estimate: x,P% (5) return to step (3) and repeat%% INITIALIZATION:%% If an initial state estimate is unavailable, it can be obtained% from the first observation as follows, provided that there are the% same number of observable variables as state variables. This "auto-% intitialization" is done automatically if s.x is absent or NaN.%% x = inv(H)*z% P = inv(H)*R*inv(H')%% This is mathematically equivalent to setting the initial state estimate% covariance to infinity.function s = kalmanf(s)% set defaults for absent fields:if ~isfield(s,'x'); s.x=nan*z; endif ~isfield(s,'P'); s.P=nan; endif ~isfield(s,'z'); error('Observation vector missing'); endif ~isfield(s,'u'); s.u=0; endif ~isfield(s,'A'); s.A=eye(length(x)); endif ~isfield(s,'B'); s.B=0; endif ~isfield(s,'Q'); s.Q=zeros(length(x)); endif ~isfield(s,'R'); error('Observation covariance missing'); endif ~isfield(s,'H'); s.H=eye(length(x)); endif isnan(s.x)   % initialize state estimate from first observation   if diff(size(s.H))      error('Observation matrix must be square and invertible for state autointialization.');   end   s.x = inv(s.H)*s.z;   s.P = inv(s.H)*s.R*inv(s.H'); else   % This is the code which implements the discrete Kalman filter:   % Prediction for state vector and covariance:   s.x = s.A*s.x + s.B*s.u;   s.P = s.A * s.P * s.A' + s.Q;   % Compute Kalman gain factor:   K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);   % Correction based on observation:   s.x = s.x + K*(s.z-s.H*s.x);   s.P = s.P - K*s.H*s.P;   % Note that the desired result, which is an improved estimate   % of the sytem state vector x and its covariance P, was obtained   % in only five lines of code, once the system was defined. (That's   % how simple the discrete Kalman filter is to use.) Later,   % we'll discuss how to deal with nonlinear systems.endreturn该程序中使用的符号的含义与本文一致,函数前的注释再清晰不过了,就不多说。下面是一段测试代码:
% Define the system as a constant of 12 volts:clear alls.x = 12;s.A = 1;% Define a process noise (stdev) of 2 volts as the car operates:s.Q = 2^2; % variance, hence stdev^2% Define the voltimeter to measure the voltage itself:s.H = 1;% Define a measurement error (stdev) of 2 volts:s.R = 2^2; % variance, hence stdev^2% Do not define any system input (control) functions:s.B = 0;s.u = 0;% Do not specify an initial state:s.x = nan;s.P = nan;% Generate random voltages and watch the filter operate.tru=[]; % truth voltagefor t=1:20   tru(end+1) = randn*2+12;   s(end).z = tru(end) + randn*2; % create a measurement   s(end+1)=kalmanf(s(end)); % perform a Kalman filter iterationendfigurehold ongrid on% plot measurement data:hz=plot([s(1:end-1).z],'r.');% plot a-posteriori state estimates:hk=plot([s(2:end).x],'b-');ht=plot(tru,'g-');legend([hz hk ht],'observations','Kalman output','true voltage',0)title('Automobile Voltimeter Example')hold offKalman的参数中s.Q和s.R的设置非常重要,之前也提过,一般要通过实验统计得到,它们分布代表了状态空间估计的误差和测量的误差。

Kalman滤波器的效果是使输出变得更平滑,但没办法去除信号中原有的椒盐噪声,而且,Kalman滤波器也会跟踪这些椒盐噪声点,因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。
返回列表