西蒙—最优状态估计卡尔曼H∞及非线性滤波（中、英PDF含代码）

西蒙—最优状态估计,卡尔曼,H∞及非线性滤波(中、英PDF含代码)

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
      

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       89785  2017-11-02 17:39  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\AppendixC.pdf
     文件      162423  2017-11-02 17:40  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\errata.pdf
     文件      231841  2017-11-02 17:43  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\ESDNonlinear.pdf
     文件      442575  2017-11-02 17:43  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\hinfinity.pdf
     文件      435748  2017-11-02 17:42  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\kalman.pdf
     目录           0  2018-07-27 23:00  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\
     文件       10137  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\AddHinfConstr.m
     文件        3881  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\AddHinfConstrMonte.m
     文件        1291  2017-11-02 17:47  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\AddHinfEx1.m
     文件        5816  2017-11-02 17:47  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\AddHinfEx3.m
     文件         969  2017-11-02 17:45  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\Chemical.m
     文件        3981  2017-11-02 17:46  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\Colored.m
     文件        2078  2017-11-02 17:46  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\ContEx.m
     文件        2849  2017-11-02 17:45  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\Correlated.m
     文件        1435  2017-11-02 17:45  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\DiscreteKFAlt.m
     文件        1997  2018-03-14 10:53  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\DiscreteKFEx1.m
     文件        1840  2017-11-02 17:45  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\DiscreteKFEx2.m
     文件         899  2017-11-02 17:45  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\DiscreteKFEx2Plot.m
     文件        3000  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\ExtendedBody.m
     文件        4848  2017-11-02 17:46  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\FixIntSmooth.m
     文件        4469  2017-11-02 17:46  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\FixLagSmooth.m
     文件        5058  2017-11-02 17:46  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\FixPtSmooth.m
     文件         360  2017-11-02 17:47  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HinfContEx1a.m
     文件        2887  2017-11-02 17:47  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HinfContEx1b.m
     文件         508  2017-11-02 17:47  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HinfEx1a.m
     文件        1719  2018-07-27 22:23  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HinfEx1b.m
     文件         972  2017-11-02 17:49  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\House1.m
     文件       14260  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\Hybrid2.m
     文件        3106  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HybridBody.m
     文件        8046  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HybridSimplex.m
     文件        5132  2017-11-02 17:48  西蒙—最优状态估计卡尔曼H∞及非线性滤波（中英）\MATLAB\HybridUKF.m
............此处省略20个文件信息