% GSSM_BFT  General state space model for Bearings and Frequency Tracking of a randomly maneuvering%           target relative to a moving observer (submarine).%%   The following state space model is used :%%     X(k) = |1 4 0 0 0| X(k-1) + | 8  0  0| V(k-1)%            |0 1 0 0 0|          | 4  0  0|%            |0 0 1 4 0|          | 0  8  0|%            |0 0 0 1 0|          | 0  4  0|%            |0 0 0 0 1|          | 0  0  1|%    (the measures are given by the sonar every 4 seconds)%%   Where the state vector is defined as%            - the 2D position and velocity vector of the target (relative to a fixed external reference frame),%            - the pure tone frequency emitted by the target (very stable).%%     X(k) = |x1(k)| = |      x-position at time k     |%            |x2(k)|   |      x-velocity at time k     |%            |x3(k)|   |      y-position at time k     |%            |x4(k)|   |      y-velocity at time k     |%            |x5(k)|   | pure tone frequency at time k |%%   And the observations at time k, O(k) are :%           - the bearing angle (in radians) from the moving observer (submarine) towards the target,%           - the doppler-shifted frequency tone tracked by the observer (submarine).%%     O(k) = |                bearing = arctan((x3(k)-sub3(k))/(x1(k)-sub1(k)))                            |  +  | v1(k) |%            |   frequency = x5(k)*(1+1/1500*((x2(k)-sub2(k))*cos(bearing)+(x4(k)-sub4(k))*sin(bearing)))  |     | v2(k) |%%   c=1500 m/s (sound speed in water)%%   The submarine state is known at each time precisely and described by the following vector :%     sub(k) = |sub1(k)| = |  x-position at time k     |%              |sub2(k)|   |  x-velocity at time k     |%              |sub3(k)|   |  y-position at time k     |%              |sub4(k)|   |  y-velocity at time k     |%              |sub5(k)|   |  frequency tone at time k | (not used here)%%   The state dynamics are driven by a 2 dimensional white Gaussian noise source and the%   observations are corrupted by additive scalar white Gaussian noise.%%   See :  Gordon, Salmond & Ewing, "Bayesian State Estimation for Tracking and Guidance Using%   the Bootstrap Filter", Journal of Guidance, Control and Dynamics, 1995.%%   This example is courtesy of Alain Bonnot.%%   Copyright  (c) Rudolph van der Merwe (2002)%%   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 by contacting%   rvdmerwe@ece.ogi.edu.  Businesses wishing to obtain a copy of the software should%   contact ericwan@ece.ogi.edu for commercial licensing information.%%   See LICENSE (which should be part of the main toolkit distribution) for more%   detail.%=============================================================================================function [varargout] = model_interface(func, varargin)  switch func    %--- Initialize GSSM data structure --------------------------------------------------------    case 'init'      model = init(varargin);        error(consistent(model,'gssm'));               % check consistentency of initialized model      varargout{1} = model;    %--------------------------------------------------------------------------------------------    otherwise      error(['Function ''' func ''' not supported.']);  end%===============================================================================================function model = init(init_args)  model.type = 'gssm';                              % object type = generalized state space model  model.tag  = 'GSSM_Bearings_Frequency_Tracking';  % ID tag  model.statedim   = 5;                      %   state dimension  model.obsdim     = 2;                      %   observation dimension  model.paramdim   = 12;                     %   parameter dimension                                             %   parameter estimation will be done)  model.U1dim      = 0;                      %   exogenous control input 1 dimension  model.U2dim      = 5;                      %   exogenous control input 2 dimension  model.Vdim       = 3;                      %   process noise dimension  model.Ndim       = 2;                      %   observation noise dimension  model.ffun_type = 'lti';                   % state transition function type  : linear time invariant  model.hfun_type = 'nla';                   % state observation function type : nonlinear, aditive noise  model.ffun      = @ffun;                   % file handle to FFUN  model.hfun      = @hfun;                   % file handle to HFUN  model.prior     = @prior;  model.likelihood = @likelihood;            % file handle to LIKELIHOOD  model.innovation = @innovation;            % file handle to INNOVATION  model.setparams  = @setparams;             % file handle to SETPARAMS  model.obsAngleCompIdxVec = [1];            % indicate that the first (and only component) of the observation                                             % vector is an angle measured in radians. This is needed so that the                                             % SPKF based algorithms can correctly deal with the angular discontinuity                                             % at +- pi radians.  Arg.type = 'gaussian';  Arg.dim = model.Vdim;  Arg.mu  = zeros(Arg.dim,1);  Arg.cov_type = 'full';  Arg.cov   = [((1e-3)^2)*eye(Arg.dim-1) zeros(Arg.dim-1,1);zeros(1,Arg.dim-1) (1e-4)^2];  model.pNoise = gennoiseds(Arg);            % process noise : zero mean white Gaussian noise, cov = (1e-3)^2(dynamics) and (1e-4)^2 (tone frequency, very stable)  Arg.type = 'gaussian';  Arg.dim = model.Ndim;  Arg.mu = zeros(Arg.dim,1);  Arg.cov_type ='full';  Arg.cov  = [0.0175^2 0;0 0.06^2];  model.oNoise = gennoiseds(Arg);            % observation noise : zero mean white Gaussian noise, cov=0.0175^2 (bearings=1