Optimal State Estimation（最优状态估计）

Dan Simon的经典大作《Optimal State Estimation》对应的Matlab源程序。包含了基本的线性Kalman滤波、EKF、UKF、粒子滤波等基本算法。适合学习滤波理论的人员。

function Colored（flag corr）

% Kalman filter simulation with colored （time correlated） measurement noise.
% INPUTS:
%    flag = 0 means ignore the time correlation of the measurement noise
%           1 means augment the system state
%           2 means use the Bryson/Henrikson approach
%    corr = magnitude of measurement noise correlation <= 1

kf = 500; % Number of time steps in simulation
phi = [0.7 -0.15; 0.03 0.79];
H = [1 0; 0 1];
psi = [corr 0; 0 corr];
L = [0.15; 0.21];
Q = L * [1] * L‘;
Qv = [0.05 0; 0 0.05];

phi1 = [phi zeros（22）; zeros（22） psi];
Q1 = [Q zeros（22）; zeros（22） Qv];
R1 = zeros（22）;
H1 = [H eye（22）];

if （flag == 0）
   n = 2; % number of states
   R = Qv;
   stitle = ‘Correlation Ignored‘;
elseif （flag == 1）
   n = 4; % number of states
   stitle = ‘Augmented State‘;
elseif （flag == 2）
   n = 2; % number of states
   R = H * Q * H‘ + Qv;
   M = Q * H‘;
   D = H * phi - psi * H;
   stitle = ‘Bryson and Henrikson‘;
else
   disp（‘illegal input argument‘）;
end

% Assume xhat（0） = x（0） = 0.
x = zeros（41）; xhatplus = zeros（n1）; xhatminus = zeros（n1）;
z = zeros（21）;
% Assume P（0） = 0.
Pplus = zeros（nn）; Pminus = zeros（nn）;

xArray = []; xhatArray = []; KArray = []; PArray = []; zArray = [];

randn（‘state‘ 0）;
for k = 1 : kf
   % Simulate the system
   StateNoise = randn;
   n（11） = L（11） * StateNoise;
   n（21） = L（21） * StateNoise;
   n（31） = sqrt（Qv（11）） * randn;
   n（41） = sqrt（Qv（22）） * randn;
   x = phi1 * x + n;
   zold = z;
   z = H1 * x;
   % Run the Kalman filter
   if （flag == 0）
       % Ignore the time correlation
      Pminus = phi * Pplus * phi‘ + Q;
      K = inv（H * Pminus * H‘ + R）;
      K = （Pminus * H‘） * K;
      xhatminus = phi * xhatplus;
      xhatplus = xhatminus + K * （z - H * xhatminus）;
      Pplus = Pminus - K * H * Pminus;
   elseif （flag == 1）
       % Use the augmented state approach
      Pminus = phi1 * Pplus * phi1‘ + Q1;
      K = inv（H1 * Pminus * H1‘ + R1）;
      K = （Pminus * H1‘） * K;
      xhatminus = phi1 * xhatplus;
      xhatplus = xhatminus + K * （z - H1 * xhatminus）;
      Pplus = Pminus - K * H1 * Pminus;
   elseif （flag == 2）
       % Use the Bryson/Henrikson approach
      zeta = z - psi * zold;
      C = M * inv（D * Pminus * D‘ + R）;
      K = Pminus * D‘ * inv（D * Pminus * D‘ + R）;
      xhatplus = xhatminus + K * （zeta - D * xhatminus）;
      xhatminus = phi * xhatplus + C * （zeta - D * xhatplus）;
      Pplus = （eye（2） - K * D） * Pminus * （eye（2） - K * D）‘ + K * R * K‘;
      Pminus = phi * Pplus * phi‘ + Q - C * M‘ - phi * K * M - M‘ * K‘ * phi‘;
      xhatplus = xhatminus;
   end
   % Save data for plotting.
   xArray = [xArray x];
   xhatArray = [xhatArray xhatplus];
   KArray = [KArray K];
   PArray = [PArray Pplus];
   zArray = [zArray z];
end

% Plot.
k = 1 : kf;
close all;
figure;
plot（k zArray（1:） - xArray（1:） ‘-‘ k xArray（1:） - xhatArray（1:） ‘

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

     文件     442575  2007-04-28 22:20  OptimalStateEstimation\hinfinity.pdf

     文件     435748  2007-04-28 22:23  OptimalStateEstimation\kalman.pdf

     文件       3106  2007-04-28 22:18  OptimalStateEstimation\HybridBody.m

     文件       4828  2007-04-28 22:15  OptimalStateEstimation\MotorSim.m

     文件       5132  2007-04-28 22:18  OptimalStateEstimation\HybridUKF.m

     文件       1172  2007-04-28 22:16  OptimalStateEstimation\LinearSimEx1.m

     文件       2917  2007-04-28 22:18  OptimalStateEstimation\UnscentedEx.m

     文件       1435  2007-04-28 22:17  OptimalStateEstimation\DiscreteKFAlt.m

     文件       5816  2007-04-28 22:18  OptimalStateEstimation\ParticleEx2.m

     文件       1998  2007-04-28 22:17  OptimalStateEstimation\DiscreteKFEx1.m

     文件       3582  2007-04-28 22:18  OptimalStateEstimation\ParticleEx1.m

     文件       1840  2007-04-28 22:17  OptimalStateEstimation\DiscreteKFEx2.m

     文件       7676  2007-04-28 22:18  OptimalStateEstimation\ParticleEx4.m

     文件        899  2007-04-28 22:17  OptimalStateEstimation\DiscreteKFEx2Plot.m

     文件       7992  2007-04-28 22:18  OptimalStateEstimation\ParticleEx5.m

     文件       3836  2007-04-28 22:17  OptimalStateEstimation\Colored.m

     文件       6115  2007-04-28 22:19  OptimalStateEstimation\ParticleEx3.m

     文件     231841  2007-04-28 22:21  OptimalStateEstimation\ESDNonlinear.pdf

     文件       2849  2007-04-28 22:17  OptimalStateEstimation\Correlated.m

     文件       2078  2007-04-28 22:18  OptimalStateEstimation\ContEx.m

     文件       7741  2007-04-28 22:18  OptimalStateEstimation\KalmanConstrained.m

     文件       5058  2007-04-28 22:18  OptimalStateEstimation\FixPtSmooth.m

     文件       3213  2007-04-28 22:18  OptimalStateEstimation\FixLagSmooth.m

     文件       3069  2007-04-28 22:18  OptimalStateEstimation\Multiple.m

     文件       4192  2007-04-28 22:18  OptimalStateEstimation\FixIntSmooth.m

     文件       6755  2007-04-28 22:18  OptimalStateEstimation\Reduced.m

     文件       5057  2007-04-28 22:18  OptimalStateEstimation\Robust.m

     文件       2095  2007-04-28 22:18  OptimalStateEstimation\Schmidt.m

     文件       3000  2007-04-28 22:18  OptimalStateEstimation\ExtendedBody.m

     文件       4633  2007-04-28 22:18  OptimalStateEstimation\MotorKalman.m

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