Previous Next t_general_noisy_jump.m

ckbs_t_general Jump Tracking Example and Test

Syntax
[ok] = affine_ok(draw_plotconVarconFreqproc_ind,
meas_ind)


draw_plot
If this argument is true, a plot is drawn showing the results

conVar
This argument gives the variance of the contaminating signal. The larger the variance, the bigger the outliers.

conFreq
This argument gives outlier frequency. For example, .1 means 1/10 measurements will be an outlier.

proc_ind
This selects which process indices the smoother will model using Student's t. The choices are [], 1, 2, and 1:2.

meas_ind
This selects which measurement indices the smoother will model using Student's t. The choices are [] and 1.

ok
If the return value ok is true, the test passed, otherwise the test failed.

State Vector
 x1 (t) derivative of function we are estimating
 x2 (t) value of function we are estimating

Measurement
 z1 (t) value of  x2 (t) plus noise

Source Code
 


function [ok]= t_general_noisy_jump(draw_plot, conVar, conFreq, proc_ind, meas_ind)

noise = 'normal';

if(nargin < 5)
    conFreq = 0;
    conVar = 0;
    proc_ind = 1:2;
    meas_ind = [];
end
% --------------------------------------------------------

N     = 100;        % number of measurement time points

dt    = 4*pi / N;  % time between measurement points

%dt = 1;

gamma =  1;        % transition covariance multiplier

sigma =  0.5;       % standard deviation of measurement noise

if (strcmp(noise,'student'))
    sigma = sqrt(2);
end


max_itr = 100;      % maximum number of iterations

epsilon = 1e-3;    % convergence criteria

h_min   = 0;       % minimum horizontal value in plots

h_max   = 14;       % maximum horizontal value in plots

v_min   = -10.0;    % minimum vertical value in plots

v_max   = 10.0;    % maximum vertical value in plots

% ---------------------------------------------------------


if nargin < 1

     draw_plot = false;

end


% number of measurements per time point
m     = 1;
% number of state vector components per time point
n     = 2;

params.level = 1;



% simulate the true trajectory and measurement noise

t1       =  (1 : N/2 - 2) * dt;
t2       =  (N/2 - 1: N/2 + 1)*dt;
t3       =  (N/2 +2 : N)*dt;

t = [t1, t2, t3];

% alpha = 8;
% beta = 4;
% gammaOne = 20;

jump = 10;
derivs = jump/(4*dt) * ones(1,3);

    t2 = [-cos(t1), derivs, -cos(t3)];
    t1 = [-jump/2-sin(t1), -jump/2-sin(N/2 - 2) + cumsum(derivs)*dt, jump/2-sin(t3)];

   x1_true = t1(1:N);
   x2_true = t2(1:N);



x_true  = [ x1_true ; x2_true ];

%
params.direct_h_index = 1;
h_fun = @(k,x) direct_h(k,x,params);

params.pos_vel_g_dt = .01;
params.pos_vel_g_initial = x_true(:,1); % initialize to true values for now
g_fun = @(k,x) pos_vel_g(k,x,params);

df_meas = 4;
df_proc = 4;
params.df_proc = df_proc;
params.df_meas = df_meas;
params.inds_proc_st = proc_ind;
params.inds_meas_st = meas_ind;

% measurement values and model


%exp_true = random('exponential', 1/sigma, 1, N);
%bin = 2*random('binomial', 1, 0.5, 1, N) - 1;
%exp_true = exp_true .* bin;
%z        = x2_true + exp_true;


% Normal Noise

v_true = zeros(1,N);
%temp = zeros(1,N);
%bin = zeros(1,N);

randn('state', sum(100*clock))
rand('twister', sum(100*clock));


if (strcmp(noise,  'normal'))
    v_true  = sigma * randn(1, N);
end


%Laplace Noise
if (strcmp(noise,'laplace'))
    temp = sigma*exprnd(1/sigma, 1,100);
    bin  = 2*(binornd(1, 0.5, 1, 100)-0.5);
    v_true = temp.*bin/sqrt(2);
end

if (strcmp(noise,'student'))
  v_true = trnd(4,1,100);
end

nI= binornd(1, conFreq, 1, N);
newNoise = sqrt(conVar)*randn(1,N);


z = x1_true + v_true + nI.*newNoise;

rk      = sigma * sigma;

rinvk   = 1 / rk;

rinv    = zeros(m, m, N);


for k = 1 : N
    if (mod(k, 2) == 0)
        rinv(:, :, k) = rinvk;
   end
end

% transition model

qinv    = zeros(n, n, N);
qk      = gamma * [ dt , dt^2/2 ; dt^2/2 , dt^3/3 ];
%qk       = gamma * [ dt^3/3 , dt^2/2 ; dt^2/2 , dt ];
qinvk   = inv(qk);

for k = 2 : N
     qinv(:,:, k)  = qinvk;
end

qinv(:,:,1) = 100*eye(n);
%


xZero = zeros(n, N);
% L1 Kalman-Bucy Smoother

w = 1;
[xOut, info] = ckbs_t_general(g_fun, h_fun, max_itr, epsilon, xZero, z, qinv, w*rinv, params);

ok = info(end, 3) < epsilon || info(1) < 1e-8;


% % --------------------------------------------------------------------------
% 
if draw_plot

     figure(1);

     clf

     hold on

     plot(t', x_true(1,:)', '-k', 'Linewidth', 2, 'MarkerEdgeColor', 'k' );

     meas = z(1,:);
     meas(meas > v_max) = v_max;
     meas(meas < v_min) = v_min;
     rvec = rinv(1,:);
     meas(rvec==0) = v_max + 5; % take non-measurements out
     plot(t', meas, 'bd',  'MarkerFaceColor', [.49 1 .63], 'MarkerSize', 6);

     %zWrong = z.*nI;
     %i = 0;
     %for(i = 1:N)
     %    if (zWrong(1,i) ~=0)
     %       plot(t(i)', zWrong(1,i)', 'rs',  'MarkerFaceColor', [.49 1 .63], 'MarkerSize', 4);
     %    end
     %end

     %plot(t', xOut(2,:)', 'r-' );
      
     
   %plot(t', xfilt(2,:)', '-b', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
     
   plot(t', xOut(1,:)', '-m', 'Linewidth', 2, 'MarkerEdgeColor', 'k' );

%   plot(t', xOut2(2,:)', '-r', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
     
%   plot(t', xOut3(2,:)', '-y', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
 
%   plot(t', xOut4(2,:)', '-g', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
 
%   plot(t', xOut5(2,:)', '-b', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
 
   
   %plot(t', xsmooth(2,:)', '-m', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
  
     %plot(t', xfilt(2,:)', '-b', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
 
%     plot(t', - ones(N,1), 'b-');

%     plot(t', ones(N,1), 'b-');

%    xlabel('Time', 14);
 %   ylabel('Position', 'Fontsize', 14);
  %  set(gca,'FontSize',14);
     

  axis([h_min, h_max, v_min, v_max]);

  res_x = zeros(n, N);
  xk = zeros(n, 1);
  for k = 1 : N
      xkm   = xk;
      xk    = xOut(:, k);
      gk    = feval(g_fun, k, xkm);
      res_x(:,k)  = xk - gk;
  end
  
%   hold off
%   figure(2);
%   clf
%   hold on
%   plot(t', x_true(2,:)', '-k', 'Linewidth', 2, 'MarkerEdgeColor', 'k' );
%   plot(t', xOut(2,:), '-m', 'Linewidth', 2, 'MarkerEdgeColor', 'k' );
%   hold off
%   
%   figure(3);
%   clf 
%   hold on
%   
%   plot(t', res_x(1,:)', '-m', 'Linewidth', 2, 'MarkerEdgeColor', 'k' );
% 
%   hold off
% 
%   figure(4)
%   clf
%   hold on
%   plot(t', res_x(2,:)', '-b', 'Linewidth', 2, 'MarkerEdgeColor', 'k' );
%   hold off
  
  
end


Input File: example/t_general_noisy_jump.m