threefreedom.m

扩展卡尔曼滤波算法

% ====================== three freedom state filter (extern kalman filter) ===========================

Method = 5;                % Select filter method (1-5):
                           % 1=EKF, 2=DD1, 3=DD1 (mex-file), 4=DD2, 5=DD2 (mex)
xfunc = 'xMissile';           % File containing state equations
yfunc = 'yMissile';           % File containing output equations
linfunc = 'lMissile';         % File containing the linearization

x0 = [3e5;2e4;1e-3];       % 初始状态矢量Initial state vector
Q  = [zeros(3)];           % 系统噪声方差Covariance of process noise
r  = 1e4;                  % 测量噪声方差Covariance of measurement noise
P0 = diag([1e6 4e6 1e-4]); % 状态估计初始方差Initial covariance on state estimate
gamma = 5e-5;              % 待定 Model parameter
M     = 1e5;               % 待定 Horizontal radar position (ft)
H     = 1e5;               % 待定 Vertical radar position (ft)
rksteps = 64;              % 龙格－库它计算步数/采样周期  RK steps / sampling period
delta = 1/rksteps;         % 快速“采样周期”Fast "Sampling period"
runs  = 50;                % 蒙特卡罗仿真次数Monte carlo repetitions


% ---- Generate test data ----
randn('seed',0);           % 设置随机数种子 Set seed for random noise
Tfinal = 60;               % 仿真终值时间 Simulate 'Tfinal' seconds
ysim   = zeros(Tfinal,1);  % 存储y变量真值  Store true y sequence
xtrue  = [x0';zeros(Tfinal,3)]; %待定
xhatmat= zeros(Tfinal+1,3,runs);%待定
v      = zeros(3,1);       % 没有系统噪声 No process noise
w0     = 0;                % 测量噪声均值 Mean of measurement noise
clear optpar
optpar.init = [delta M H gamma]; % 准备初始化参数 Prepare initialization parameters
optpar.F = eye(3);
optpar.G=1;

% 运行仿真 Run the simulation
x  = x0;
xMissile(optpar.init);              % 初始化状态 Initialze state update
yMissile(optpar.init);              % 初始化观测方程 Initialize observation equation
for k=1:Tfinal,
  for kk=1:1/delta,
     x=xMissile(x,[],v);
  end
  xtrue(k+1,:) = x';
  ysim(k)=xMissile(x,w0);
end

%产生蒙特卡罗仿真观测数据 Generate 'run' different data sets
ytrue = repmat(ysim,1,runs) + sqrt(r)*randn(Tfinal,runs);


%----- 进行蒙特卡罗变换 Do the Monte Carlo shit -----
x0hat = [x0(1:2);3e-5];        % Initial state estimate
idx   = [1:Tfinal]'*rksteps;   % Measurement time stamps (in rk-periods)

[v,d] = eig(P0);           % Cholesky factor of initial state covariance
Sx0 = real(v*sqrt(d));
[v,d] = eig(Q);            % Cholesky factor of process noise covariance
Sv    = real(v*sqrt(d));
[v,d] = eig(r);            % Cholesky factor of measurement noise covariance
Sw    = real(v*sqrt(d));

for k=1:runs,
fprintf('\nExperiment no. %d\n',k);

  %----- Estimate state trajectory -----
  switch Method
     case 1,
       [xhat,Pmat]=ekf(xfunc,yfunc,linfunc,x0hat,P0,Q,r,[],ytrue(:,k),idx,optpar);
     case 2,
       [xhat,Smat]=dd1(xfunc,yfunc,x0hat,P0,Q,r,[],ytrue(:,k),idx,optpar);
     case 3
       [xhat,Smat]=dd1fall(x0hat,Sx0,Sv,Sw,[],ytrue(:,k),idx,optpar);
     case 4,
       [xhat,Smat]=dd2(xfunc,yfunc,x0hat,P0,Q,r,[],ytrue(:,k),idx,optpar);
     case 5,
       [xhat,Smat]=dd2fall(x0hat,Sx0,Sv,Sw,[],ytrue(:,k),idx,optpar);
     otherwise
        error('No valid filter method selected. Method=1...5')
  end
  
  % ----- Store data -----
  xhatmat(:,:,k) = xhat([1;(idx+1)],:);
end
xhatmean = mean(xhatmat,3);    % Calculate mean values


%----- Display Results -----
close all
figure(1)
subplot(111)
plot(0:Tfinal,abs(xtrue(:,1)-xhatmean(:,1))); axis([0 60 0 300])
ylabel('Absolute value of average altitude error (ft)');
xlabel('Time (sec)');

figure(2)
plot(0:Tfinal,abs(xtrue(:,2)-xhatmean(:,2))); axis([0 60 0 350])
ylabel('Absolute value of average velocity error (ft)');
xlabel('Time (sec)');

figure(3)
semilogy(0:Tfinal,abs(xtrue(:,3)-xhatmean(:,3))); axis([0 60 1e-9 1e-2])
ylabel('Absolute value of average error in ballistic coefficient');
xlabel('Time (sec)');