%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         |                                                      %
%      DC |                                                      %
%      D  C                                                      %
%      D  | C       CoDE - Control, Dynamics and Estimation      %
%  ----D--+---C--------------------------------------------->    %
%      D  | C           University at Buffalo, New York          %
%      D  C                                                      %
%      DC |                                                      %
%         |http://code.eng.buffalo.edu                           %
%         |Dirk Tenne                                            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DISCLAIMER:
% May this Matlab code help you to understand the concept of the
% uncsented filter. Feel free to use it any way you'd like and I am
% glad that I could help you out, or saved you some time of typing.
% Enjoy!


% main file to generate polar measurements and convert them to the
% cartesian coordinate system. We compare the Monte Carlo results
% with the unscented transformation for various settings.

% the truth
  r_true  = 1e-0; % 1m 
  p_true  = 0;    % 0 degree
  rp_true = [r_true p_true]';
  xy_true = polar2cart(rp_true);

% noise characteristics:
  r_dev = 4e-2;      % 4cm
  p_dev = 15*pi/180; % 15 degree
  R = [r_dev^2 0; 0 p_dev^2];

% monte carlo simulation
  N = 1000;
  [xy_mean,xy_var] = monte_carlo('polar2cart',rp_true,R,N)

% true debiased solution [Lerro, Bar-Shalom]
  [xy_debias,P_debias] = debias_lerro(rp_true,R); %conditioned on
                                                  %the measurement 
  [xy_debias2,P_debias2] = debias_lerro(rp_true,R,rp_true);

% standard unscented prediction
  method = 'symmetric';
  moments = [rp_true diag(R) zeros(2,1) 3*diag(R).^2]
  [xy_ukf,P_ukf] = houf('polar2cart',moments,method)

% higher order filter
  method = 'gut2b';
  moments = [rp_true diag(R) zeros(2,1) 3*diag(R).^2]
  [xy_gut,P_gut] = houf('polar2cart',moments,method)
 
