matlab UKF 状态估计软件包

matlab代码，利用UKF滤波器进行状态估计的算法

function [ErrKarray ErrKCarray ErrHinfarray ErrHinfCarray] = AddHinfConstr（g T tf）

% function AddHinfConstr
% This m-file simulates a vehicle tracking problem.
% The vehicle state is estimated with a minimax filter.
% In addition with the a priori knowledge that the vehicle is on
% a particular road the vehicle state is estimated with a 
% constrained minimax filter.
% This m-file also simulates a Kalman filter and constrained
% Kalman filter so you can compare results.
% The state consists of the north and east position and the
% north and east velocity of the vehicle.
% The measurement consists of north and east position.
% For further details see the web site 
% http://www.csuohio.edu/simond/minimaxconstrained/.
% INPUTS
%   g = gamma （I suggest 40）
%   T = time step in seconds （I suggest 1）
%   tf = final time in seconds （I suggest 120）
% OUTPUTS
%   ErrKarray = time varying array of error of Kalman unconstrained state estimate
%   ErrKCarray = time varying array of error of Kalman constrained state estimate
%   ErrHinfarray = time varying array of error of Minimax unconstrained state estimate
%   ErrHinfCarray = time varying array of error of Minimax constrained state estimate

if ~exist（‘g‘ ‘var‘）
    g = 40;
end
if ~exist（‘T‘ ‘var‘）
    T = 1;
end
if ~exist（‘tf‘ ‘var‘）
    tf = 120;
end

Q = diag（[4 4 1 1]）; % Process noise covariance （m m m/sec m/sec）
Qsqrt = sqrt（Q）;

R = diag（[900 900]）; % Measurement noise covariance （m m）
Rsqrt = sqrt（R）;

theta = pi / 3; % heading angle （measured CCW from east）
tantheta = tan（theta）;

% Define the initial state x initial unconstrained Kalman filter estimate xhat
% and initial constrained Kalman filter estimate xtilde.
x = [0; 0; tantheta; 1] * 100;
xhat = x;
xtilde = x;
P = diag（[R（11） R（22） Q（11） Q（22）]）; % Initial estimation error covariance

% AccelDecelFlag is used to simulate the vehicle alternately accelerating and
% decelerating as if in traffic.
AccelDecelFlag = 1;

% System matrix.
A = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1];

% Input matrix.
B = [0; 0; T*sin（theta）; T*cos（theta）];

% Normalized measurement matrix.
C = inv（Rsqrt） * [1 0 0 0; 0 1 0 0];

% State constraint matrices.
D = [1 -tantheta 0 0; 0 0 1 -tantheta];
% Normalize D so that D*D‘=I.
D = D / sqrt（1 + tantheta^2）;
V = D‘ *  D;
d = [0; 0];

% Initialize arrays for saving data for plotting.
xarray = [];
xhatarray = [];
xtildearray = [];
randn（‘state‘ sum（100*clock））;

% Minimax initialization.
% Make sure that xtildeinf satisfies the state constraint.
Qbar = P; 
Qtilde = P;
xhatinf = x;
xtildeinf = x;
xhatinfarray = [];
xtildeinfarray = [];

for t = T : T : tf
    
    % Get the noise-corrupted measurement z.
    z = C * x;
    MeasErr = randn（size（z））;
    z = z + MeasErr;
    
    % Set the known input u.
    if AccelDecelFlag == 1
        if （x（3） > 30） | （x（4） > 30）
            AccelDecelFlag = -1;
        end
    else
      

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      10137  2008-10-22 17:22  matlab code for optimal state estimation\AddHinfConstr.m

     文件       3881  2008-10-22 17:22  matlab code for optimal state estimation\AddHinfConstrMonte.m

     文件       1291  2008-10-22 17:22  matlab code for optimal state estimation\AddHinfEx1.m

     文件       5816  2008-10-22 17:22  matlab code for optimal state estimation\AddHinfEx3.m

     文件        969  2008-10-22 17:21  matlab code for optimal state estimation\Chemical.m

     文件       3836  2008-10-22 17:21  matlab code for optimal state estimation\Colored.m

     文件       2078  2008-10-22 17:21  matlab code for optimal state estimation\ContEx.m

     文件       2849  2008-10-22 17:21  matlab code for optimal state estimation\Correlated.m

     文件       1507  2010-01-04 10:33  matlab code for optimal state estimation\DiscreteKFAlt.asv

     文件       1522  2010-01-04 10:37  matlab code for optimal state estimation\DiscreteKFAlt.m

     文件       2057  2010-01-04 10:53  matlab code for optimal state estimation\DiscreteKFEx1.m

     文件       1840  2008-10-22 17:21  matlab code for optimal state estimation\DiscreteKFEx2.m

     文件        899  2008-10-22 17:21  matlab code for optimal state estimation\DiscreteKFEx2Plot.m

     文件       3000  2008-10-22 17:22  matlab code for optimal state estimation\ExtendedBody.m

     文件       4192  2008-10-22 17:21  matlab code for optimal state estimation\FixIntSmooth.m

     文件       3213  2008-10-22 17:21  matlab code for optimal state estimation\FixLagSmooth.m

     文件       5058  2008-10-22 17:21  matlab code for optimal state estimation\FixPtSmooth.m

     文件        360  2008-10-22 17:22  matlab code for optimal state estimation\HinfContEx1a.m

     文件       2887  2008-10-22 17:22  matlab code for optimal state estimation\HinfContEx1b.m

     文件        508  2008-10-22 17:21  matlab code for optimal state estimation\HinfEx1a.m

     文件       1685  2008-10-22 17:22  matlab code for optimal state estimation\HinfEx1b.m

     文件      13698  2008-10-22 17:22  matlab code for optimal state estimation\Hybrid2.m

     文件       3107  2011-04-13 23:10  matlab code for optimal state estimation\HybridBody.m

     文件       5213  2010-01-04 10:58  matlab code for optimal state estimation\HybridUKF.asv

     文件       5132  2010-01-04 11:01  matlab code for optimal state estimation\HybridUKF.m

     文件       7741  2008-10-22 17:21  matlab code for optimal state estimation\KalmanConstrained.m

     文件       1172  2008-10-22 17:21  matlab code for optimal state estimation\LinearSimEx1.m

     文件       4633  2008-10-22 17:22  matlab code for optimal state estimation\MotorKalman.m

     文件       4828  2008-10-22 17:20  matlab code for optimal state estimation\MotorSim.m

     文件       3069  2008-10-22 17:21  matlab code for optimal state estimation\Multiple.m

............此处省略14个文件信息