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
