./build_doc.sh

 


function [ok]= t_general_ok(draw_plot, conVar, conFreq, noise)


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

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 = 1:2;
params.inds_meas_st = [];

% 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;
     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]);


  hold off

end

 


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

 
      [fk, Fk] = f_fun(k, xk)
      n  = size(xk, 1);
      fk = -1;
      Fk =  zeros(1, n);
      return
      end

 
	test_path
	get_started_ok(true)

 
function [ok] = get_started_ok(draw_plot)
    if nargin < 1
        draw_plot = false;
    end
    % --------------------------------------------------------------------------
    max_itr = 20;      % maximum number of iterations
    epsilon = 1e-5;    % convergence criteria
    N       = 40;      % number of time points
    n       = 1;       % number of state vector components per time point
    sigma_r = 1;       % variance of measurement noise
    sigma_q = 1;       % variance of transition noise
    sigma_e = 1;       % variance of initial state estimate
    initial = ones(n); % estimate for initial state value
    randn('seed', 4321)
    % ---------------------------------------------------------------------------
    % Level of tracing during optimization
    if draw_plot
        level = 1;
    else
        level = 0;
    end
    m       = n;       % number of measurements per time point
    ell     = 2 * n;   % number of constraints
    index   = 1 : n;   % index vector
    % ---------------------------------------------------------------------------
    %  information used by box_f
    params.box_f_lower = 0.5 * ones(n, 1);
    params.box_f_upper = 1.5 * ones(n, 1);
    params.box_f_index = index;
    % ---------------------------------------------------------------------------
    % global information used by persist_g
    params.persist_g_initial = initial;
    % ---------------------------------------------------------------------------
    % global information used by direct_h
    %global direct_h_info
    
    params.direct_h_index = index;
     h_fun = @(k,x) direct_h(k,x,params);
    % ---------------------------------------------------------------------------
    % simulated true trajectory 
    x_true  = ones(n, N);
    % initial vector during optimization process
    x_in    = 0.5 * ones(n, N);
    % measurement variances
    rinv    = zeros(m, m, N);
    qinv    = zeros(n, n, N);
    z       = zeros(m, N);
    for k = 1 : N
        xk           = x_true(:, k);
        ek           = randn(m, 1);
        hk           = h_fun(k, xk);
        z(:, k)      = hk + ek;
        qinv(:,:, k) = eye(n) / (sigma_q * sigma_q);
        rinv(:,:, k) = eye(n) / (sigma_r * sigma_r);
    end
    qinv(:,:,1)       = eye(n) / (sigma_e * sigma_e);
    % ----------------------------------------------------------------------
    % call back functions
    f_fun = @(k,xk) box_f(k,xk,params);
    g_fun = @(k,xk) persist_g(k,xk,params);
   
    % ----------------------------------------------------------------------
    % call optimizer
    [x_out, u_out, info] = ckbs_nonlinear( ...
        f_fun,    ...
        g_fun,    ...
        h_fun,    ...
        max_itr,  ...
        epsilon,  ...
        x_in,     ...
        z,        ...
        qinv,     ...
        rinv,     ...
        level     ...
    );
    % ----------------------------------------------------------------------
    ok     = size(info, 1) <= max_itr;
    %
    % Set up affine problem corresponding to x_out (for a change in x).
    % Check constraint feasibility and complementarity.
    f_out  = zeros(ell, N);
    g_out  = zeros(n,   N);
    h_out  = zeros(m,   N);
    df_out = zeros(ell, n, N);
    dg_out = zeros(n, n,   N);
    dh_out = zeros(m, n,   N);
    xk1    = zeros(n, 1);
    for k = 1 : N
        xk       = x_out(:, k);
        uk       = u_out(:, k);
        [fk, Fk] = f_fun(k, xk);
        [gk, Gk] = g_fun(k, xk1);
        [hk, Hk] = h_fun(k, xk);
        %
        ok       = ok & all( fk <= epsilon);              % feasibility
        ok       = ok & all( abs(fk) .* uk <= epsilon );  % complementarity
        %
        f_out(:, k)    = fk;
        g_out(:, k)    = gk - xk;
        h_out(:, k)    = hk;
        df_out(:,:, k) = Fk;
        dg_out(:,:, k) = Gk;
        dh_out(:,:, k) = Hk;
        xk1 = xk;
    end
    ok     = ok & all( all( u_out >= 0 ) );
    %
    % Compute gradient of unconstrained affine problem at dx equal to zero
    % and then check the gradient of the Lagrangian
    dx_out = zeros(n, N);
    grad  = ckbs_sumsq_grad(dx_out, z, g_out, h_out, dg_out, dh_out, qinv, rinv);
    for k = 1 : N
        uk  = u_out(:, k);
        Fk  = df_out(:,:, k);
        dk  = grad(:, k);
        %
        ok  = ok & all( abs(Fk' * uk + dk) <= epsilon );
    end
    % ----------------------------------------------------------------------
    if draw_plot
        figure(1);
        clf
        hold on
        time = 1 : N;
        plot(time, x_true(1,:),    'b-');
        plot(time, x_out(1,:),     'g-');
        plot(time, z(1,:),         'ko'); 
        plot(time, 0.5*ones(1,N),  'r-');
        plot(time, 1.5*ones(1,N),  'r-');
        title('Position: blue=truth, green=estimate, red=constraint, o=data');
        axis( [ 0 , N , -1, 3 ] )
        hold off
    return
end

 
function [ok] = sine_wave_ok(constraint, draw_plot)
    % --------------------------------------------------------
    % Simulation problem parameters
    max_itr    = 100;             % maximum number of iterations
    epsilon    = 1e-5;           % convergence criteria
    N          = 50;             % number of measurement time points
    dt         = 2 * pi / N;     % time between measurement points
    sigma_r    = .5;             % standard deviation of measurement noise
    sigma_q    = 1;              % square root of multiplier for transition variance
    sigma_e    = 100;            % standard deviation           
    delta_box  = 0.;             % distance from truth to box constraint
    delta_sine = .1;             % distance from truth to sine constraint
    h_min      = 0;              % minimum horizontal value in plots
    h_max      = 7;              % maximum horizontal value in plots
    v_min      = -1.5;           % minimum vertical value in plots
    v_max      = +1.5;           % maximum vertical value in plots 
    a          = [ 0    ; -1.5]; % position that range one measures from
    b          = [ 2*pi ; -1.5]; % position that range two measures to
    randn('seed', 1234);         % Random number generator seed
    % ---------------------------------------------------------
    % level of tracing during the optimization
    if draw_plot
            level   = 1;
    else
            level   = 0;
    end
    % ---------------------------------------------------------
    % simulate the true trajectory and measurement noise
    t        =  (1 : N) * dt;         % time
    x_true   = [ ones(1, N) ; t ; cos(t) ; sin(t) ];
    initial  = x_true(:,1);
    % ---------------------------------------------------------
    % information used by box_f
    params.box_f_lower = - 1 - delta_box;
    params.box_f_upper = + 1 + delta_box;
    params.box_f_index = 4;
    % ---------------------------------------------------------
    % information used by sine_f

    params.sine_f_index  = [ 2 ; 4 ];
    params.sine_f_offset = [ 0 ; delta_sine ];
    % ---------------------------------------------------------
    % information used by pos_vel_g
    params.pos_vel_g_dt       = dt;
    params.pos_vel_g_initial  = initial;
    g_fun     = @(k,x) pos_vel_g(k,x,params);

    % ---------------------------------------------------------
    %  information used by distance_h
   
    params.distance_h_index    = [ 2 ; 4 ];
    params.distance_h_position = [a , b ];
    h_fun = @(k,x) distance_h(k,x, params);
    % ---------------------------------------------------------
    % problem dimensions
    m     = 2; % number of measurements per time point
    n     = 4; % number of state vector components per time point
    ell   = 0; % number of constraint (reset to proper value below)
    % ---------------------------------------------------------
    % simulate the measurements
    [c,k_max] = max(x_true(4,:));    % index where second position component is max
    rinv      = zeros(m, m, N);      % inverse covariance of measurement noise
    z         = zeros(m, N);         % measurement values
    randn('state',sum(100*clock));   % random input
    for k = 1 : N
            x_k   = x_true(:, k);
            if k == k_max
                    % special case to increase chance that upper constraint is active 
                    x_k(2)   = x_k(2) + .5;
                    h_k      = h_fun(k, x_k);
                    z(:, k)  = h_k; 
            else
                    % simulation according to measurement model
                    h_k      = h_fun(k, x_k);
                    z(:, k)  = h_k + sigma_r * randn(2, 1); 
            end
            rinv(:,:, k) = eye(m) / (sigma_r * sigma_r);
    end

    %fid = fopen('z4.dat', 'wt');
    %fprintf(fid, '%6.6f %6.6f\n', z);
    z = load('z3.dat')';

    % ---------------------------------------------------------
    % inverse transition variance
    qk       = sigma_q * sigma_q * [ ...
            dt       , dt^2 / 2 , 0 , 0 
            dt^2 / 2 , dt^3 / 3 , 0 , 0 
            0        , 0        , dt       , dt^2 / 2
            0        , 0        , dt^2 / 2 , dt^3 / 3
    ];
    qinvk    = inv(qk);
    qinv     = zeros(n, n, N);
    for k = 2 : N
            qinv(:,:, k) = qinvk;
    end
    %
    % inverse initial estimate covariance
    qinv(:,:,1) = eye(n) * sigma_e * sigma_e; 
    % ----------------------------------------------------------
    % initialize x vector at position and velocity zero
    x_in = zeros(n, N);
    % ----------------------------------------------------------------------
   
    %
    is_no_f   = false;
    is_box_f  = false;
    is_sine_f = false;
    switch constraint
            case {'no_constraint'}
            ell       = 1;
            f_fun     = @ no_f;
            is_no_f   = true;

            case {'box_constraint'}
            ell       = 2;
            f_fun     = @(k,xk) box_f(k,xk,params);
            is_box_f  = true;

            case {'sine_constraint'}
            ell       = 1;
            f_fun     = @(k,xk) sine_f(k,xk,params);
            is_sine_f = true;

            otherwise
            constraint = constraint
            error('sine_wave_ok: invalid value for constraint')
    end
    % ----------------------------------------------------------------------
    [x_out, u_out, info] = ckbs_nonlinear( ...
            f_fun,    ...
            g_fun,    ...
            h_fun,    ...
            max_itr,  ...
            epsilon,  ...
            x_in,     ...
            z,        ...
            qinv,     ...
            rinv,     ...
            level     ...
    );
    % ----------------------------------------------------------------------
    ok         = true;
    ok         = ok & (size(info,1) <= max_itr);
    %
    % Set up affine problem (for change in x) corresponding to x_out.
    % Check constraint feasibility and complementarity.
    f_out      = zeros(ell, N);
    g_out      = zeros(n, N);
    h_out      = zeros(m, N);
    df_out     = zeros(ell, n, N);
    dg_out     = zeros(n, n, N);
    dh_out     = zeros(m, n, N);
    xk1        = zeros(n, 1);
    for k = 1 : N
            xk    = x_out(:, k);
            uk    = u_out(:, k);
            [fk, Fk]   = f_fun(k, xk);
            [gk, Gk]   = g_fun(k, xk1);
            [hk, Hk]   = h_fun(k, xk);
            %
            ok   = ok & all( fk <= epsilon );              % feasibility
            ok   = ok & all( abs(fk) .* uk <= epsilon );   % complementarity
            %
            f_out(:, k)    = fk;
            g_out(:, k)    = gk - xk;
            h_out(:, k)    = hk;
            df_out(:,:, k) = Fk;
            dg_out(:,:, k) = Gk;
            dh_out(:,:, k) = Hk;
            %
            xk1 = xk;
    end
    ok   = ok & all( all( u_out >= 0 ) );
    %
    % Compute gradient of unconstrained affine problem at zero
    % and check gradient of Lagrangian
    dx_out = zeros(n, N);
    d_out  = ckbs_sumsq_grad(dx_out, z, g_out, h_out, dg_out, dh_out, qinv, rinv);
    for k = 1 : N
            uk = u_out(:, k);
            Fk = df_out(:,:, k);
            dk = d_out(:, k);
            %
            ok = ok & (max ( abs( Fk' * uk + dk ) ) <= 1e2*epsilon);
    end
    %
    if draw_plot
            figure(1)
            clf
            hold on
            plot( x_true(2,:) , x_true(4,:) , 'b-' );
            plot( x_out(2,:)  , x_out(4,:)  , 'g-' );
            if is_no_f
                    title('blue=truth, green=estimate, no constraint')
            end
            if is_box_f
                    title('blue=truth, green=estimate, red=linear constraint')
                    plot( t , + ones(1, N) + delta_box , 'r-'); 
                    plot( t , - ones(1, N) - delta_box , 'r-'); 
            end
            if is_sine_f
                    title('blue=truth, green=estimate, red=nonlinear constraint')
                    plot( x_out(2,:) , sin( x_out(2,:) ) + delta_sine , 'r-'); 
            end
            axis( [ h_min, h_max, v_min, v_max] );
    return
end

 
function [ok] = vanderpol_ok(draw_plot)
    if nargin < 1
        draw_plot = false;
    end
    % ---------------------------------------------------------------
    % Simulation problem parameters
    max_itr = 20;           % Maximum number of optimizer iterations
    epsilon = 1e-4;         % Convergence criteria
    N       = 41;           % Number of measurement points (N > 1)
    T       = 4.;           % Total time
    ell     = 1;            % Number of constraints (never active)
    n       = 2;            % Number of components in the state vector (n = 2)
    m       = 1;            % Number of measurements at each time point (m <= n)
    xi      = [ 0. , -5.]'; % True initial value for the state at time zero
    x_in    = zeros(n, N);  % Initial sequence where optimization begins
    initial = [ 0. , -5.]'; % Estimate of initial state (at time index one)
    mu      = 2.0;          % ODE nonlinearity parameter
    sigma_r = 1.;           % Standard deviation of measurement noise
    sigma_q = .1;           % Standard deviation of the transition noise
    sigma_e = 10;           % Standard deviation of initial state estimate
    randn('seed', 4321);    % Random number generator seed
    % -------------------------------------------------------------------
    % Level of tracing during optimization
    if draw_plot
        level = 1;
    else
        level = 0;
    end
    % spacing between time points
    dt = T / (N - 1);
    % -------------------------------------------------------------------
    %  information used by vanderpol_g
    params.vanderpol_g_initial  = initial;
    params.vanderpol_g_dt       = dt;
    params.vanderpol_g_mu       = mu;
    % -------------------------------------------------------------------
    % global information used by direct_h
    %global direct_h_info
    params.direct_h_index = 1;
    % ---------------------------------------------------------------
    % Rest of the information required by ckbs_nonlinear
    %
    % Step size for fourth order Runge-Kutta method in vanderpol_sim
    % is the same as time between measurement points
    step    = dt;
    % Simulate the true values for the state vector
    x_true  = vanderpol_sim(mu, xi, N, step);
    time    = linspace(0., T, N);
    % Simulate the measurement values
    z       = x_true(1:m,:) + sigma_r * randn(m, N);
    % Inverse covariance of the measurement and transition noise
    rinv    = zeros(m, m, N);
    qinv    = zeros(n, n, N);
    for k = 1 : N
        rinv(:, :, k) = eye(m, m) / sigma_r^2;
        qinv(:, :, k) = eye(n, n) / sigma_q^2;
    end
    qinv(:, :, 1) = eye(n, n) / sigma_e^2;
    % ---------------------------------------------------------------
    % call back functions
    f_fun = @ no_f;
    g_fun = @ (k,x) vanderpol_g(k,x,params);
    h_fun = @(k,x) direct_h(k,x,params);
    % ---------------------------------------------------------------
    % call the optimizer
    [ x_out , u_out, info ] = ckbs_nonlinear( ...
        f_fun ,    ...
        g_fun ,    ...
        h_fun ,    ...
        max_itr ,  ...
        epsilon ,  ...
        x_in ,     ...
        z ,        ...
        qinv ,     ...
        rinv ,     ...
        level      ...
        );
    % ----------------------------------------------------------------------
    % Check that x_out is optimal
    % (this is an unconstrained case, so there is no need to check f).
    ok     = size(info, 1) <= max_itr;
    g_out  = zeros(n,   N);
    h_out  = zeros(m,   N);
    dg_out = zeros(n, n,   N);
    dh_out = zeros(m, n,   N);
    xk     = zeros(n, 1);
    for k = 1 : N
        xkm      = xk;
        %
        xk       = x_out(:, k);
        uk       = u_out(:, k);
        [gk, Gk] = g_fun(k, xkm);
        [hk, Hk] = h_fun(k, xk);
        %
        g_out(:, k) = gk - xk;
        h_out(:, k) = hk;
        dg_out(:,:, k) = Gk;
        dh_out(:,:, k) = Hk;
    end
    dx    = zeros(n, N);
    grad  = ckbs_sumsq_grad(dx, z, g_out, h_out, dg_out, dh_out, qinv, rinv);
    ok    = max( max( abs(grad) ) ) < epsilon;
    % ----------------------------------------------------------------------
    if draw_plot
        figure(1);
        clf
        hold on
        plot(time, x_true(1,:), 'b-');
        plot(time, x_out(1,:),  'g-');
        plot(time, z(1,:),      'ko');
        title('Position: blue=truth, green=estimate, o=data');
        hold off
        return
    end
end

 
function [ok] = vanderpol_sim_ok()
    mu     = 1.;
    xi     = [ 2 ; 0 ];
    n_out  = 21;
    step   = .1;
    x_out  = vanderpol_sim(mu, xi, n_out, step);
    t_sol  = [ 0.0    , 0.5    , 1.0    ,  1.5   , 2.0    ];
    x1_sol = [ 2.0000 , 1.8377 , 1.5081 ,  1.0409, 0.3233 ];
    ok     = 1;
    for k = 1 : 5
        ok = ok & abs( x_out(1, (k-1)*5 + 1) - x1_sol(k) ) < 1e-4;
    end
    return
end

 
function [gk, Gk] = vanderpol_g(k, xk1, params)
    initial = params.vanderpol_g_initial;
    dt      = params.vanderpol_g_dt;
    mu      = params.vanderpol_g_mu;
    n       = 2;
    if (size(xk1,1) ~= n) | (size(xk1,2) ~= 1)
        size_xk1_1 = size(xk1, 1)
        size_xk1_2 = size(xk1, 2)
        error('vanderpol_g: xk1 not a column vector with two elements')
    end
    if (size(initial,1) ~= n) | (size(initial,2) ~= 1)
        size_initial_1 = size(initial, 1)
        size_initial_2 = size(initial, 2)
        error('vanderpol_g: initial not a column vector with two elements')
    end
    if (size(dt,1)*size(dt,2) ~= 1) | (size(mu,1)*size(mu,2) ~= 1)
        size_dt_1 = size(dt, 1)
        size_dt_2 = size(dt, 2)
        size_mu_1 = size(mu, 1)
        size_mu_2 = size(mu, 2)
        error('vanderpol_g: dt or mu is not a scalar')
    end
    if k == 1
        gk = initial;
        Gk = zeros(n, n);
    else
        gk      = zeros(n, 1);
        gk(1)   = xk1(1) + xk1(2) * dt;
        gk(2)   = xk1(2) + (mu * (1 - xk1(1)*xk1(1)) * xk1(2) - xk1(1)) * dt;
        %
        Gk      = zeros(n, n);
        Gk(1,1) = 1;
        Gk(1,2) = dt;
        Gk(2,1) = (mu * (- 2 * xk1(1)) * xk1(2) - 1) * dt;
        Gk(2,2) = 1 + mu * (1 - xk1(1)*xk1(1)) * dt;
    end
    return
end

 
function [fk, Fk] = box_f(k, xk, params)
    index = params.box_f_index;
    lower = params.box_f_lower;
    upper = params.box_f_upper;
    ell    = 2 * size(lower, 1);
    n      = size(xk,    1);
    %
    if (size(lower, 2) ~= 1) | (size(upper, 2) ~= 1) | ...
            (size(xk, 2) ~= 1)
        size_lower_2 = size(lower, 2)
        size_upper_2 = size(upper, 2)
        size_index_2 = size(index, 2)
        size_xk_2    = size(xk,    2)
        error('box_f: either lower, upper, index or xk not a column vector')
    end
    if (2 * size(upper, 1) ~= ell) | (2 * size(index, 1) ~= ell)
        size_lower_1 = size(lower, 1)
        size_upper_1 = size(upper, 1)
        size_index_1 = size(index, 1)
        error('box_f: lower, upper, or index has a different size')
    end
    if (max(index) > n) | (min(index) < 1)
        max_index = max(index)
        min_index = min(index)
        error('box_f: max(index) > size(xk, 1) or min(index) < 1')
    end
    %
    fk = zeros(ell, 1);
    Fk = zeros(ell, n);
    for p = 1 : (ell / 2)
        j                = index(p);
        fk(2 * p - 1, 1) = xk(j) - upper(p);
        Fk(2 * p - 1, j) = +1.;
        %
        fk(2 * p,     1) = lower(p) - xk(j);
        Fk(2 * p,     j) = -1.;
    end
    return
end

 
function [hk, Hk] = direct_h(k, xk, params)
    index = params.direct_h_index;
    m        = size(index, 1);
    n        = size(xk, 1);
    if (size(xk, 2)~=1) | (size(index,2)~=1)
        size_xk_2    = size(xk, 2)
        size_index_2 = size(index, 2)
        error('direct_h: xk or index is not a column vector')
    end
    if (max(index) > n) | (min(index) < 1)
        max_index = max(index)
        min_index = min(index)
        error('direct_h: max(index) > size(xk, 1) or min(index) < 1')
    end
    hk  = xk(index);
    Hk  = zeros(m, n);
    for i = 1 : m
        Hk( i, index(i) ) = 1;
    end
    return
end

 
function [hk, Hk] = distance_h(k, xk, params)
	index    = params.distance_h_index;
	position = params.distance_h_position;
	n        = size(xk, 1);
	m        = size(position, 2);
	if (size(xk, 2)~=1) | (size(index,2)~=1)
		size_xk_2    = size(xk, 2)
		size_index_2 = size(index, 2)
		error('distance_h: xk or index is not a column vector')
	end
	if size(position,1) ~= size(index,1)
		size_position_1 = size(position, 1)
		size_index_1    = size(index, 1)
		error('distance_h: position and index have different number of rows')
	end
	hk  = zeros(m, 1);
	Hk  = zeros(m, n);
	p_x = xk(index);
	for i = 1 : m
		p_i          = position(:, i);
		hk(i)        = norm( p_x - p_i );
		if hk(i) ~= 0
			Hk(i, index) = (p_x - p_i) / hk(i);
		end
	end
	return
end

 
function [fk, Fk] = no_f(k, xk)
    % no constraint case: f(x) = -1 for all x
    n  = size(xk, 1);
    fk = -1;
    Fk = zeros(1, n);
    return
end

 
function [gk, Gk] = persist_g(k, xk1, params)
    initial = params.persist_g_initial;
    n       = size(xk1, 1);
    if k == 1
        gk = initial;
        Gk = zeros(n, n);
    else
        gk = xk1;
        Gk = eye(n);
    end
    return
end

 
function [gk, Gk] = pos_vel_g(k, xk1, params)
    dt      = params.pos_vel_g_dt;
    initial = params.pos_vel_g_initial;
    n  = size(xk1, 1);
    %
    if (size(xk1, 2)~=1) | (size(initial, 1)~=n) | (size(initial, 2)~=1)
        size_xk1_1     = size(xk1, 1)
        size_initial_1 = size(initial, 1)
        size_initial_2 = size(initial, 2)
        error('pos_vel_g: initial or xk1 not column vectors with same size')
    end
    %
    Gk = zeros(n, n);
    if k == 1
        gk = initial;
        return;
    end
    % gk(2*j-1) = xk1(2*j-1)
    Gk     = eye(n);
    for j2 = 2 : 2 : n
        % gk(2*j) = xk1(2*j) + xk1(2*j-1) * dt
        Gk(j2, j2-1) = dt;
    end
    gk = Gk * xk1;
    return
end

 
function [fk, Fk] = sine_f(k, xk, params)
    n      = size(xk, 1);
    index  = params.sine_f_index;
    offset = params.sine_f_offset;
    if (size(index, 1) ~= 2) | (size(index, 2) ~= 1)
        size_index_1 = size(index, 1)
        size_index_2 = size(index, 2)
        error('sine_f: index is not a column vector with two elements')
    end
    if (size(offset, 1) ~= 2) | (size(offset, 2) ~= 1)
        size_offset_1 = size(offset, 1)
        size_offset_2 = size(offset, 2)
        error('sine_f: offset is not a column vector with two elements')
    end
    if (max(index) > n) | (min(index) < 1)
        max_index = max(index)
        min_index = min(index)
        error('sine_f: max(index) > size(xk, 1) or min(index) < 1')
    end
    fk = xk( index(2) ) - sin( xk( index(1) + offset(1) ) ) - offset(2);
    Fk           = zeros(1, n);
    Fk(index(2)) = 1;
    Fk(index(1)) = Fk(index(1)) - cos( xk( index(1) + offset(1) ) );
    return
end

 

function [ok]= L1_nonlinear_ok(draw_plot)


    ok = 1;
    conVar = 100;
    conFreq = .2;

    numComp = 1;
    noise = 'normal';

    if nargin < 1
        draw_plot = false;
    end
    % ---------------------------------------------------------------
    % Simulation problem parameters
    max_itr = 100;           % Maximum number of optimizer iterations
    epsilon = 1e-6;         % Convergence criteria
    N       = 41;           % Number of measurement points (N > 1)
    T       = 4.;           % Total time
    ell     = 1;            % Number of constraints (never active)
    n       = 2;            % Number of components in the state vector (n = 2)
    m       = 1;            % Number of measurements at each time point (m <= n)
    xi      = [ 0. , -.5]'; % True initial value for the state at time zero
    x_in    = zeros(n, N);  % Initial sequence where optimization begins
    % SASHA: changed 0, .5 to 0 0
    x_est   = [ 0 , -.5]'; % Estimate of initial state (at time index one)
    mu      = 2.0;          % ODE nonlinearity parameter
    sigma_r = 1.;           % Standard deviation of measurement noise
    sigma_q = .1;  % Standard deviation of the transition noise
    sigma_qa = .1; % 'Actual' noise

    % Step size for fourth order Runge-Kutta method in vanderpol_sim
    % is the same as time between measurement points
    step    = T / (N-1);



    % SASHA: changed .1 to 1
    sigma_e = sqrt(.1);           % Standard deviation of initial state estimate
    rand('seed', 4321);     % Random number generator seed
    % Level of tracing during optimization
    if draw_plot
        level = 1;
    else
        level = 0;
    end



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

    % Plotting parameters
    h_min   = 0;       % minimum horizontal value in plots
    h_max   = T;       % maximum horizontal value in plots
    v_min   = -5.0;    % minimum vertical value in plots
    v_max   = 5.0;    % maximum vertical value in plots

   %________________
   f_fun = @ no_f;

    
    % -------------------------------------------------------------------
    % information used by vanderpol_g
    params.vanderpol_g_initial  = xi;
    params.vanderpol_g_dt       = step;
    params.vanderpol_g_mu       = mu;
    g_fun = @(k, xk) vanderpol_g(k, xk, params);
    % -------------------------------------------------------------------
    % information used by direct_h
    
    params.direct_h_index = 1;
    h_fun = @(k,x) direct_h(k,x,params);
    % ---------------------------------------------------------------

  
    % ---------------------------------------------------------------
    % Rest of the information required by ckbs_nonlinear
    %
    % Simulate the true values for the state vector
    x_vdp  = vanderpol_sim(mu, xi, N, step);
    time    = linspace(0., T, N);

    % x_trueLong = zeros(n, 10*N);
    % x_true = zeros(n, N);
    % vanderpol_info.N = 10*N;
    % x_trueLong(:,1) = xi; % + sigma_e*randn(n, 1);
    % for k = 2:10*N
    %     x_trueLong(:,k) = vanderpol_g(k, x_trueLong(:,k-1)) + sqrt(.1*sigma_q)*randn(n,1);
    % end
    % x_true = x_trueLong(:, [1 10:10:10*N-10]);
    % vanderpol_info.N = N;

    x_true = zeros(n, N);
    x_true(:,1) = xi;% + .1*sigma_e*randn(n, 1);
    for k = 2:N
        x_true(:,k) = g_fun(k, x_true(:,k-1)) + sigma_qa*randn(n,1);
    end

    % Inverse covariance of the measurement and transition noise
    rinv    = zeros(m, m, N);
    qinv    = zeros(n, n, N);
    for k = 1 : N
        rinv(:, :, k) = eye(m, m) / sigma_r^2;
        qinv(:, :, k) = eye(n, n) / sigma_q^2;
    end
    qinv(:, :, 1) = eye(n, n) / sigma_e^2;
    % call back functions

    v_true = zeros(m,N);
    temp = zeros(m,N);
    bin = zeros(n,N);


    iter = 0;
    while(iter <= numComp)
        iter = iter + 1;
        if(mod(iter, 100) == 0)
            fprintf('iter conVar conFreq\n');
            fprintf('%d %1.4f %1.4f\n', iter, conVar, conFreq);
        end

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

        switch(noise)
            case{'normal'}
                v_true  = sigma_r * randn(m, N);

            case{'laplace'}
                temp = sigma_r*exprnd(1/sigma_r, m, N);
                bin  = 2*(binornd(1, 0.5, m, N)-0.5);
                v_true = temp.*bin/sqrt(2);

            case{'student'}
                v_true = trnd(4,m,N);
        end

        % Create contaminating distribution

        nums = rand(1, N);
        biNums = nums < conFreq;
        %    nI= ones(m,1)*binornd(1, conFreq, 1, N); % contaminate entire measurement, not just componentwise
        nI = ones(m,1)*biNums;
        newNoise = sqrt(conVar)*randn(m,N); % Contaminating noise

        %M = 50;

        %newNoise = unidrnd(M, 1, N) - M/2;

        % Simulate the measurement values
        %measurement = x_true(1:m,:).*x_true(1:m, :); % measurement is x^2
        measurement =x_true(1,:); % measurement is x^2

        z       =  measurement + v_true + nI.*newNoise;
        %addNoise = v_true + nI.*newNoise;
        %z       = addNoise + x_true(1, :) + 0.5*(x_true(2,:) - 0).^2  ;
        %z1 = addNoise(1, :) + x_true(1, :);s
        %z2 = addNoise(2, :) + 0.5*x_true(1, :).*x_true(1, :) + 0.5*x_true(2,:).*x_true(2,:);
        %z = [z1; z2];
        %z = addNoise + (1/3)^3*(x_true(2,:)).^3 + x_true(2);


        % ---------------------------------------------------------------
        % call the optimizer
        %addpath('../src');


        if(draw_plot)
            ckbs_level = 0;
            [ x_out_Gauss , u_out, info ] = ckbs_nonlinear( ...
                f_fun ,    ...
                g_fun ,    ...
                h_fun ,    ...
                max_itr ,  ...
                epsilon ,  ...
                x_in ,     ...
                z ,        ...
                qinv ,     ...
                rinv ,     ...
                ckbs_level      ...
                );
        end
        [ x_out_L1 , rOut, sOut, pPlusOut, pMinusOut, info ] = ckbs_L1_nonlinear( ...
            f_fun ,    ...
            g_fun ,    ...
            h_fun ,    ...
            max_itr ,  ...
            epsilon ,  ...
            x_in ,     ...
            z ,        ...
            qinv ,     ...
            rinv ,     ...
            level      ...
            );

        ok = ok && info(end, 3) < epsilon;

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


        figure(1);

        clf

        subplot(2, 1,1)
        hold on



        plot(time, x_vdp(1,:), 'r-.', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
        plot(time, x_true(1,:), 'k-', 'Linewidth', 3.5, 'MarkerEdgeColor', 'k');
        plot(time, x_out_Gauss(1,:), 'b--', 'Linewidth', 3.5, 'MarkerEdgeColor', 'k');
        plot(time, x_out_L1(1,:), 'm-.', 'Linewidth', 3.5, 'MarkerEdgeColor', 'k');



        meas = z(1,:);
        meas(meas > v_max) = v_max;
        meas(meas < v_min) = v_min;
        %plot(time, z(1,:), 'o');
        plot(time, meas, 'bo',  'MarkerFaceColor', [.49 1 .63], 'MarkerSize', 6.2);

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

        hleg = legend('vdp solution', 'stochastic realization', 'Gaussian Smoother', 'L1 Smoother', 'Data', 'Location', 'NorthWest');
        xlabel('Time (s)', 'FontSize', 16);
        ylabel('X-component', 'FontSize', 16);
        set(hleg, 'Box', 'off');
        set(gca,'FontSize',16);
        set(hleg, 'FontSize', 14);
        title('20% of measurement errors are N(0, 100)');


        %plot(time, z(1,:), 'o');

        hold off

        subplot(2,1, 2)
        hold on

        plot(time, x_vdp(2,:), 'r-.', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
        plot(time, x_true(2,:), 'k-', 'Linewidth', 3.5, 'MarkerEdgeColor', 'k');
        plot(time, x_out_Gauss(2,:), 'b--', 'Linewidth', 3.5, 'MarkerEdgeColor', 'k');
        plot(time, x_out_L1(2,:), 'm-.', 'Linewidth', 3.5, 'MarkerEdgeColor', 'k');
        axis([h_min, h_max, v_min, v_max]);

        hleg = legend('vdp solution', 'stochastic realization', 'Gaussian Smoother', 'L1 Smoother', 'Location', 'NorthWest');
        xlabel('Time(s)', 'FontSize', 16);
        ylabel('Y-component', 'FontSize', 16);
        set(hleg, 'Box', 'off');
        set(gca,'FontSize',16);
        set(hleg, 'FontSize', 14);


        hold off





        return
    end

end

 
function [ok] = affine_ok_box(draw_plot)
    % --------------------------------------------------------
    % You can change these parameters
    N     = 50;        % number of measurement time points
    dt    = 2*pi / N;  % time between measurement points
    gamma =  1;        % transition covariance multiplier
    sigma =  .5;       % standard deviation of measurement noise
    max_itr = 30;      % maximum number of iterations
    epsilon = 1e-5;    % convergence criteria
    h_min   = 0;       % minimum horizontal value in plots
    h_max   = 7;       % maximum horizontal value in plots
    v_min   = -2.0;    % minimum vertical value in plots
    v_max   = +2.0;    % maximum vertical value in plots
    % ---------------------------------------------------------
    ok = true;
    if nargin < 1
        draw_plot = false;
    end
    %  Define the problem
    rand('seed', 1234);
    %
    % number of constraints per time point
    ell   = 4;
    %
    % number of measurements per time point
    m     = 1;
    %
    % number of state vector components per time point
    n     = 2;
    %
    % simulate the true trajectory and measurement noise
    t       =  (1 : N) * dt;
    x1_true = - cos(t);
    x2_true = - sin(t);
    x_true  = [ x1_true ; x2_true ];
    v_true  = sigma * rand(1, N);
    %
    % measurement values and model
    v_true  = sigma * randn(1, N);
    z       = x2_true + v_true;
    rk      = sigma * sigma;
    rinvk   = 1 / rk;
    rinv    = zeros(m, m, N);
    h       = zeros(m, N);
    dh      = zeros(m, n, N);
    for k = 1 : N
        rinv(:, :, k) = rinvk;
        h(:, k)       = 0;
        dh(:,:, k)    = [ 0 , 1 ];
    end
    %
    % transition model
    g       = zeros(n, N);
    dg      = zeros(n, n, N);
    qinv    = zeros(n, n, N);
    qk      = gamma * [ dt , dt^2/2 ; dt^2/2 , dt^3/3 ];
    qinvk   = inv(qk);
    for k = 2 : N
        g(:, k)       = 0;
        dg(:,:, k)    = [ 1 , 0 ; dt , 1 ];
        qinv(:,:, k)  = qinvk;
    end
    %
    % initial state estimate
    g(:, 1)      = x_true(:, 1);
    qinv(:,:, 1) = 100 * eye(2);
    %
    % constraints
    b       = zeros(ell, N);
    db      = zeros(ell, n, N);
    for k = 1 : N
        %
        % b(:, k) + db(:,:, k) * x(:, k) <= 0
        b(:, k)    = [ -1 ; -1 ; -1 ; -1 ];
        db(:,:, k) = [ -1 , 0 ; 1 , 0 ; 0 , -1 ; 0 , 1 ];
    end
    %
    % -------------------------------------------------------------------------
    [xOut, uOut, info] = ...
        ckbs_affine(max_itr, epsilon, z, b, g, h, db, dg, dh, qinv, rinv);
    % --------------------------------------------------------------------------
    ok   = ok & all( info(end, 1:3) <= epsilon);
    d    = ckbs_sumsq_grad(xOut, z, g, h, dg, dh, qinv, rinv);
    for k = 1 : N
        xk = xOut(:, k);
        uk = uOut(:, k);
        bk = b(:, k);
        Bk = db(:,:, k);
        dk = d(:, k);
        sk = - bk - Bk * xk;
        %
        ok = ok & (min(uk) >= 0.);
        ok = ok & (max (bk + Bk * xk) <= epsilon);
        ok = ok & (max ( abs( Bk' * uk + dk ) ) <= epsilon);
        ok = ok & (max ( uk .* sk ) <= epsilon );
    end
    if draw_plot
        figure(1);
        clf
        hold on
        plot(t', x_true(2,:)', 'r-' );
        plot(t', z(1,:)', 'ko' );
        plot(t', xOut(2,:)', 'b-' );
        plot(t', - ones(N,1), 'b-');
        plot(t', ones(N,1), 'b-');
        axis([h_min, h_max, v_min, v_max]);
        title('Constrained');
        hold off
        %
        % constrained estimate
        x_con = xOut;
    end
    %
    % Unconstrained Case
    b           = zeros(0, N);
    db          = zeros(0, n, N);
    % -------------------------------------------------------------------------
    [xOut, uOut, info] = ...
        ckbs_affine(max_itr, epsilon, z, b, g, h, db, dg, dh, qinv, rinv);
    % --------------------------------------------------------------------------
    ok   = ok & all( info(end, 1:3) <= epsilon);
    d    = ckbs_sumsq_grad(xOut, z, g, h, dg, dh, qinv, rinv);
    for k = 1 : N
        xk = xOut(:, k);
        dk = d(:, k);
        %
        ok = ok & (min(dk) <= epsilon);
    end
    if draw_plot
        figure(2);
        clf
        hold on
        plot(t', x_true(2,:)', 'r-' );
        plot(t', z(1,:)', 'ko' );
        plot(t', xOut(2,:)', 'b-' );
        plot(t', - ones(N,1), 'b-');
        plot(t', ones(N,1), 'b-');
        axis([h_min, h_max, v_min, v_max]);
        title('Unconstrained');
        hold off
        %
        % unconstrained estimate
        x_free = xOut;
        %
        % write out constrained and unconstrained results
        [fid, msg] = fopen('affine_ok_box.out', 'wt');
        if size(msg, 2) > 0 
            disp(['affine_ok: ', msg]);
        end
        %                      123456789012345678901234'
        heading =             '       t';
        heading = [ heading , ' x2_true  x2_con x2_free' ];
        heading = [ heading , '      z1\n'               ];
        fprintf(fid, heading);
        for k = 1 : N
            fprintf(fid,'%8.3f%8.3f%8.3f%8.3f%8.3f\n', ...
                t(k), ...
                x_true(2,k), x_con(2,k), x_free(2,k), ...
                z(1,k) ...
            );
        end
        fclose(fid);
    end
    return
end

 
function [ok] = affine_singular_ok(draw_plot)

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

    ok = true;
    % You can change these parameters

    conVar = 100;         % variance of contaminating distribution

    conFreq = .3;         % frequency of outliers

    noise = 'normal';     % type of noise

    N     = 1000;        % number of measurement time points

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

    %dt = 1;

    gamma =  1;        % transition covariance multiplier

    sigma =  1;       % standard deviation of measurement noise
    sigma_alg = 1;

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


    max_itr = 30;      % maximum number of iterations

    epsilon = 1e-4;    % convergence criteria

    h_min   = 0;       % minimum horizontal value in plots

    h_max   = 14;       % maximum horizontal value in plots

    v_min   = -5.0;    % minimum vertical value in plots

    v_max   = 5.0;    % maximum vertical value in plots

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


    if nargin < 1

        draw_plot = false;

    end

    %  Define the problem

    %rand('seed', 12);

    %

    % number of constraints per time point

    % ell   = 0;

    %

    % number of measurements per time point

    m     = 1;

    %

    % number of state vector components per time point

    n     = 2;

    %

    % simulate the true trajectory and measurement noise

    t       =  (1 : N) * dt;


    x1_true = - cos(t);
    x2_true = - sin(t);
    x_true  = [ x1_true ; x2_true ];

    % 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(m,N);
    temp = zeros(m,N);
    bin = zeros(m,N);

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


    if (strcmp(noise,  'normal'))
        v_true  = sigma * randn(m, 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



    z = [x2_true] + v_true;

    rk      = sigma_alg * sigma_alg * eye(m);

    rinvk   = inv(rk);

    rinv    = zeros(m, m, N);
    r       = zeros(m, m, N);

    h       = zeros(m, N);

    % Covariance between process and measurement?
    dh      = zeros(m, n, N);

    for k = 1 : N

        rinv(:, :, k) = rinvk;
        r(:,:,k) = rk;

        h(:, k)       = 0;

        dh(:,:, k)    = [ 0 , 1 ];

    end

    %

    % transition model

    g       = zeros(n, N);

    dg      = zeros(n, n, N);

    qinv    = zeros(n, n, N);
    q       = zeros(n, n, N);

    qk      = gamma * [ dt , 0 ; 0 , 0 ];

    
    qinvk   = inv(qk);

    for k = 2 : N

        g(:, k)       = 0;

        dg(:,:, k)    = [ 1 , 0 ; dt , 1 ];

        qinv(:,:, k)  = qinvk;
        
        q(:,:,k) = qk;

    end

    %

    % initial state estimate

    g(:, 1)      = x_true(:, 1);

    qinv(:,:, 1) = 100 * eye(2);
    
    q(:,:,1) = inv(qinv(:,:,1));

    %

    % constraints

    b       = zeros(0, N);

    db      = zeros(0, n, N);


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


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

    % L1 Kalman-Bucy Smoother
    % Linear unconstrained Kalman-Bucy Smoother
    [xOut2, info] = ckbs_affine_singular(z, g, h, dg, dh, q, r);
    
    %qk      = gamma * [ dt , dt^2/2 ; dt^2/2 , dt^3/3 ];

    %for k = 2:N
    %    q(:,:,k) = qk;
    %end
    
    %[xOut3, info] = ckbs_affine_singular(z, g, h, dg, dh, q, r);
    
    iter = info(5);
    info(5) = 0;

    iter
    ok = (ok) && (max(info) < epsilon);


    
    
    

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

        qk      = gamma * [ dt , dt^2/2 ; dt^2/2 , dt^3/3 ];
        
        qinvk   = inv(qk);

        for k = 2 : N
            qinv(:,:, k)  = qinvk;       
        end
        
        
        [xOut3, uOut, info] = ckbs_affine(max_itr, epsilon, z, b, g, h, db, dg, dh, qinv, rinv);
        
        figure(1);

        clf

        hold on

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

        meas = z(1,:);
        meas(meas > v_max) = v_max;
        meas(meas < v_min) = v_min;
        plot(t', meas, 'bd',  'MarkerFaceColor', [.49 1 .63], 'MarkerSize', 6);


        plot(t', xOut2(2,:)', '-r', 'Linewidth', 2, 'MarkerEdgeColor', 'k');
%        plot(t', xOut3(2,:)', '-b', 'Linewidth', 2, 'MarkerEdgeColor', 'k');


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

        title('Position Plot. Black=truth, Blue = meas, Magenta = Singular Smoother.');

        hold off

        figure(2);
        clf

        hold on

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



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

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


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

        title('Derivative plot. Black=truth, Blue = SDE smoother, Magenta = Singular Smoother.');

        
        %

        % constrained estimate

    end
   % [norm(xOut2(2,:) - x_true(2,:)) norm(xOut3(2,:) - x_true(2,:))]
end