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