aero_extkalman_L2_msfcn(block)

function aero_extkalman_L2_msfcn(block)
%MSFUNTMPL A template for an M-file S-function
%   The M-file S-function is written as a MATLAB function with the
%   same name as the S-function. 
%
%   It should be noted that the M-file S-function is very similar
%   to Level-2 C-Mex S-functions. You should be able to get more 
%   information for each of the block methods by referring to the
%   documentation for C-Mex S-functions.
%  
%   Copyright 2003-2004 The MathWorks, Inc.
%   $Revision: 1.1.6.5 $  
  setup(block);
  
%endfunction

%% Function: setup ===================================================
%% Abstract:
%%   Set up the S-function block's basic characteristics such as:
%%   - Input ports
%%   - Output ports
%%   - Dialog parameters
%%   - Options
%% 
%%   C-Mex S-function counterpart: mdlInitializeSizes
%%
function setup(block)
  
  %% Register number of dialog parameters   
  block.NumDialogPrms = 1;
  
  %% Register number of input and output ports
  block.NumInputPorts  = 1;
  block.NumOutputPorts = 2;

  % Setup port properties to be inherited or dynamic
  %block.SetPreCompInpPortInfoToDynamic;
  %block.SetPreCompOutPortInfoToDynamic;
  
  block.InputPort(1).Complexity    = 'Real'; 
  block.InputPort(1).Dimensions    = 2;
  block.OutputPort(1).Complexity   = 'Real';
  block.OutputPort(1).SamplingMode = 'Sample';
  block.OutputPort(1).Dimensions   = 2;
  block.OutputPort(2).Complexity   = 'Real';
  block.OutputPort(2).SamplingMode = 'Sample';
  block.OutputPort(2).Dimensions   = 4;
  
  block.InputPort(1).DirectFeedthrough = true;
  
  %% Set block sample time 
  block.SampleTimes = [block.DialogPrm(1).Data 0];
  
    
  block.RegBlockMethod('PostPropagationSetup',    @DoPostPropSetup);
  block.RegBlockMethod('InitializeConditions', @InitializeConditions);
  block.RegBlockMethod('Outputs',@Output);  
  block.RegBlockMethod('Update', @Update);
  
%% Run accelerator on TLC
  block.SetAccelRunOnTLC(false);
  
function DoPostPropSetup(block)
  
  %% Setup Dwork  
                         
  block.NumDworks = 3;
  block.Dwork(1).Name = 'xhat'; %% states
  block.Dwork(1).Dimensions      = 4;
  block.Dwork(1).DatatypeID      = 0;
  block.Dwork(1).Complexity      = 'Real';
  block.Dwork(1).UsedAsDiscState = true;
  
  block.Dwork(2).Name = 'P'; %% covariance matrix
  block.Dwork(2).Dimensions      = 16;
  block.Dwork(2).DatatypeID      = 0;
  block.Dwork(2).Complexity      = 'Real';
  block.Dwork(2).UsedAsDiscState = true;

  block.Dwork(3).Name = 'residual'; %% residuals
  block.Dwork(3).Dimensions      = 2;
  block.Dwork(3).DatatypeID      = 0;
  block.Dwork(3).Complexity      = 'Real';
  block.Dwork(3).UsedAsDiscState = true;

function InitializeConditions(block)

  %% Initialize Dwork 
  block.Dwork(1).Data = [0.001; 0.01; 0.001; 400];
  block.Dwork(2).Data = zeros(16,1);
  block.Dwork(3).Data = zeros(2,1);
   
%endfunction

function Output(block)

block.OutputPort(1).Data = block.Dwork(3).Data; 

block.OutputPort(2).Data = block.Dwork(1).Data;
  
%endfunction

function Update(block)

deltat=block.DialogPrm(1).Data;
Phi = [1 deltat 0 0; 0 1 0 0 ; 0 0 1 deltat; 0 0 0 1];
Q =  diag([0 .005 0 .005]);
R =  diag([300^2 0.001^2]);
meas = block.InputPort(1).Data;
xhat=block.Dwork(1).Data;
P=reshape(block.Dwork(2).Data,4,4);

% 2. Propagate the covariance matrix:
P = Phi*P*Phi' + Q;

% 3. Propagate the track estimate::
xhat = Phi*xhat;

% 4 a). Compute observation estimates:
Rangehat = sqrt(xhat(1)^2+xhat(3)^2);
Bearinghat = atan2(xhat(3),xhat(1));

% 4 b). Compute observation vector y and linearized measurement matrix M 
yhat = 	[Rangehat;
         Bearinghat];
M = [ cos(Bearinghat)          0 sin(Bearinghat)          0
     -sin(Bearinghat)/Rangehat 0 cos(Bearinghat)/Rangehat 0 ];

% 4 c).  Compute residual (Estimation Error)
residual = meas - yhat;
block.Dwork(3).Data=residual;

% 5. Compute Kalman Gain:
W = P*M'*inv(M*P*M'+ R);

% 6. Update estimate
xhat = xhat + W*residual;

% 7. Update Covariance Matrix
P = (eye(4)-W*M)*P*(eye(4)-W*M)' + W*R*W';

block.Dwork(1).Data = xhat;
block.Dwork(2).Data = P(:);
    
%endfunction