demse5.m

有关kalman滤波及其一些变形滤波算法

% DEMSE4  Bearing and Frequency Tracking Example
%
%   This demonstrates the use of the Sigma-Point Particle Filter on the classic
%   HARD bearing and frequency tracking problem.
%
%   This example is courtesy of Alain Bonnot.
%
%   See also
%   GSSM_BOT DEMSE4 DEMSE5
%   Copyright (c) Oregon Health & Science University (2006)
%
%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
%   academic use only (see included license file) and can be obtained from
%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
%   software should contact [e-mail] for commercial licensing information.
%
%   See LICENSE (which should be part of the main toolkit distribution) for more
%   detail.

%=============================================================================================

clc;
clear;

fprintf('\nDEMSE5 :  Submarine bearing and frequency tracking \n\n');
fprintf('Your submarine tries to determine the position, course, speed and emitted frequency \n');
fprintf('of a target.\n');
fprintf('At the beginning of the exercise, you know that :\n');
fprintf('           - the speed of the target is between 8 and 16 m/s ;\n');
fprintf('           - its range is between 1000 and 3000 m;\n');
fprintf('           - its frequency tone is very stable between 295 and 305 Hz.\n\n');
fprintf('You only observe the bearings of the target, and the doppler-shifted tone frequency.\n');
fprintf('At mid-course, you perform a maneuver to improve observability.\n\n');
fprintf('A random target trajectory is generated for each run, resulting in varying\n');
fprintf('tracking performance depending on the observability.\n\n\n')


%--- General setup

addrelpath('../gssm');         % add relative search path to example GSSM files to MATLABPATH
addrelpath('../data');         % add relative search path to example data files to MATLABPATH


%--- Initialise GSSM

model = gssm_bft('init');


%--- Generate inference data structure

Arg.model = model;                                   % embed GSSM
Arg.type = 'state';                                  % estimation type
Arg.tag = 'State estimation for bearings and frequency tracking problem';  % info tag (not required)

InfDS = geninfds(Arg);                               % call generate function

%--- Generate estimation process and observation noise sources

ftype = input('Inference algorithm  [ srcdkf / pf / sppf / gspf / gmsppf ] : ','s');  %  set type of inference algorithm (estimator) to use :


%--- Generate some data : Initial target state generated according to Gordon, Salmond & Ewing - 1995

T = 100;                                              % max. time k=1..T

V = model.pNoise.sample( model.pNoise, T);     % generate process noise
W = model.oNoise.sample( model.oNoise, T);     % generate observation noise

%Generate the observer (submarine) trajectory

%Initial state of submarine
sub=zeros(InfDS.statedim,T);

sub(1,1)=0;
sub(2,1)=10;
sub(3,1)=0;
sub(4,1)=0;
sub(5,1)=350;

for k=2:(T/2-1);
    sub(:,k)=model.ffun(model,sub(:,k-1),V(:,k-1),[]);
end

%At mid-course, the sub changes its course (90