Index-> contents reference index search external Previous Next Up-> ckbs ckbs_L1_nonlinear L1_nonlinear_ok.m ckbs-> license ckbs_t_general ckbs_nonlinear ckbs_L1_nonlinear ckbs_affine ckbs_affine_singular ckbs_L1_affine utility all_ok.m whatsnew wishlist bib ckbs_L1_nonlinear-> L1_nonlinear_ok.m L1_nonlinear_ok.m Headings-> Syntax draw_plot ok Van Der Pol Oscillator Simulated State Vector Transition Model and Simulated State Measurements Source Code

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