xiangmu .m

所做项目中的一个卡尔曼滤波算法

% Demonstration for Kalman filter and smoother using a 2D CWPA model
%
% Copyright (C) 2007 Jouni Hartikainen
%
% This software is distributed under the GNU General Public 
% Licence (version 2 or later); please refer to the file 
% Licence.txt, included with the software, for details.
function kf_cwpa_demo

% Transition matrix for the continous-time system.
T =  1;
A = [1 T 0 0 ;
     0 1 0 0 ;
     0 0 1 T ;
     0 0 0 1 ];

% Noise effect matrix for the continous-time system.
B = T^2/2;
L = [B 0;
     T 0;
     0 B;
     0 T;];

% Stepsize
%dt = 0.5;

% Process noise variance
%q = 0.2;
%Qc = diag([q q]);

% Discretization of the continous-time system.
Qc = (0.5^2) * eye(size(L,2));
Q=L*Qc*L';
% Measurement model.
H = [1 0 0 0 ;
     0 0 1 0 ];

% Variance in the measurements.
r1 = 900;
r2 = 400;
R = diag([r1 r2]);

% Generate the data.
n = 100;
Y = zeros(size(H,1),n);
X_r = zeros(size(A,1),n);
X_r(:,1) = [100 25 100 20 ]';
for i = 2:n
   X_r(:,i) = A*X_r(:,i-1) + L*gauss_rnd(zeros(size(H,1),1), Qc);
end

% Generate the measurements.
for i = 1:n
   Y(:,i) = H*X_r(:,i) + gauss_rnd(zeros(size(Y,1),1), R);
end

clf; clc;
disp('This is a demonstration program for the classical Kalman filter.');
disp(' ');
disp(['KF is used here to estimate the position of a moving object, whos ',...
     'dynamics follow the CWPA-model described in the documentation ',...
      'provided with the toolbox.']);
disp(' ');
disp(['We get noisy measurements from the objects position and velocity ',...
      'on discrete time steps. The real position of the object and the ',...
      'measurements made of them are displayed now. The blue line is the ',...
      'real path of the object and the green dots represents the ',...
      'measurements. The red circle represents the starting point ',...
      'of the object.']);

disp(' ');
fprintf('Filtering with KF...');

plot(X_r(1,:),X_r(3,:),Y(1,:),Y(2,:),'.',X_r(1,1),...
     X_r(3,1),'ro','MarkerSize',12);
legend('Real trajectory', 'Measurements');
title('Position');