Previous Next L1_nonlinear_ok.m

ckbs_L1_nonlinear: Robust Nonlinear Transition Model Example and Test

Syntax
[ok] = L1_nonlinear_ok(draw_plot)

draw_plot
If this optional argument is true, a plot is drawn showing the results. If draw_plot is not present or false, no plot is drawn.

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

Van Der Pol Oscillator
The test for ckbs_L1_nonlinear is based on the Van Der Pol oscillator, which is documented in the routine vanderpol_ok.m .

Simulated State Vector
We use  x1 (t) and  x2 (t) to denote the oscillator position and velocity as a function of time. The deterministic ordinary differential equation for the Van der Pol oscillator is  \[
\begin{array}{rcl}
      x1 '(t) & = & x2 (t)
      \\
      x2 '(t) & = & \mu [ 1 - x1(t)^2 ] x2 (t) - x1(t)
\end{array}
\] 


Transition Model and Simulated State
We simulate the state sequence by using Euler's approximation for the ODE with a step size of  \Delta t = .1 , together with Gaussian noise. Given  x_{k-1} , the value of the state at time  t_{k-1} , we obtain  x_k , the state value at time  t_k = t_{k-1} + \Delta t , by  \[
\begin{array}{rcl}
      x_{1,k} & = & x_{1,k-1} + x_{2,k-1} \Delta t + w_{1,k}
      \\
      x_{2,k} & = & x_{2,k-1} +
      [ \mu ( 1 - x_{1,k-1}^2 ) x_{2,k-1} - x_{1,k-1} ] \Delta t
      + w_{2,k}
\end{array}
\] 
where  w_k \sim \B{N}( 0 , Q_k ) and the variance  Q_k = \R{diag} ( 0.01, 0.01 ) . The routine vanderpol_g.m calculates the mean for  x_k given  x_{k-1} . The initial state estimate is  [0, -.5] , and its variance estimate is  Q_1 = \R{diag} ( .01 , .01 ) .

Measurements
For this example, the measurements are noisy direct observations of position  \[
\begin{array}{rcl}
      z_{1,k} & = & x_{1,k} + v_{1,k}
\end{array}
\] 
where  v_k is a mixture of nominal Gaussian noise and large outliers, so  v_k \sim 0.8 \B{N}( 0 , R_k ) + 0.2 \B{N}( 0 , 100 )  and the measurement variance is a scalar  R_k = 1 . The routine direct_h.m calculates the mean for  z_k , given the value of  x_k ; i.e.  x_{1,k} .

Source Code
 

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


Input File: example/nonlinear/L1_nonlinear_ok.m