function [ok] = nonlinear_ok_sin(draw_plot)
if nargin < 1
draw_plot = false;
end
% --------------------------------------------------------
% You can change these parameters
N = 50; % number of measurement time points
dt = 2 * pi / N; % time between measurement points
sigma = .25; % standard deviation of measurement noise
gamma = 1.; % multiplier for transition variance
max_itr = 50; % maximum number of iterations
epsilon = 1e-4; % convergence criteria
x4_min = .25; % minimum value for x4 constraint
h_min = 0; % minimum horizontal value in plots
h_max = 7; % maximum horizontal value in plots
v_min = 0; % minimum vertical value in plots
v_max = 2.5; % maximum vertical value in plots
x4_dist = .05; % distance true path is from the constraint
s1 = [ 0 , 0 ]; % station one
s2 = [ 2*pi , 0 ]; % station two
%
% level of tracing during the optimization
if draw_plot
level = 1;
else
level = 0;
end
% ---------------------------------------------------------
% global variables used by nonlinear_ok_box_h
global nonlinear_ok_box_s1
global nonlinear_ok_box_s2
nonlinear_ok_box_s1 = s1;
nonlinear_ok_box_s2 = s2;
%
% global variable used by nonlinear_ok_sin_f
global nonlinear_ok_sin_x4_min
nonlinear_ok_sin_x4_min = x4_min;
%
ok = true;
%
% Define the problem
rand('seed', 234);
%
% number of constraints per time point
ell = 1;
%
% number of measurements per time point
m = 2;
%
% number of state vector components per time point
n = 4;
%
% simulate the true trajectory and measurement noise
t = (1 : N) * dt;
x1_true = ones(1, N);
x2_true = t;
x3_true = - cos(t);
x4_true = x4_dist + 1 - sin(t) + x4_min;
x_true = [ x1_true ; x2_true ; x3_true ; x4_true ];
v1_true = sigma * randn(1, N);
v2_true = sigma * randn(1, N);
%
% corresponding measurement values
rinv = zeros(m, m, N);
z = zeros(m, N);
rinvk = eye(m) / (sigma * sigma);
for k = 1 : N
x_k = x_true(:, k);
h_k = nonlinear_ok_box_h(k, x_k);
z(:, k) = h_k + [ v1_true(k) ; v2_true(k) ];
rinv(:,:, k) = rinvk;
end
%
% covariance for the transition noise
qk = diag( [ dt, dt^3 / 3 , dt, dt^3 / 3 ] );
qk(1, 2) = dt^2 / 2;
qk(2, 1) = dt^2 / 2;
qk(3, 4) = dt^2 / 2;
qk(4, 3) = dt^2 / 2;
qk = qk * gamma;
qinvk = inv(qk);
qinv = zeros(n, n, N);
for k = 2 : N
qinv(:,:, k) = qinvk;
end
%
% covariance for the initial estimate
qinv(:,:,1) = eye(n) * 100 * gamma;
%
% initial x vector
x_in = zeros(n, N);
x_in(4, :) = 1;
%
% nonlinear_ok_sin use same g function as nonlinear_box_ok
% global variables used by nonlinear_ok_box_g
global nonlinear_ok_box_x1
global nonlinear_ok_box_dt
nonlinear_ok_box_x1 = x_true(:, 1);
nonlinear_ok_box_dt = dt;
%
% ----------------------------------------------------------------------
f_fun = 'nonlinear_ok_sin_f'; % special nonlinear_ok_sin constraints
g_fun = 'nonlinear_ok_box_g'; % same transition function as nonlinear_ok_box
h_fun = 'nonlinear_ok_box_h'; % same measurement function as nonlinear_ok_box
[x_out, u_out, info] = ckbs_nonlinear( ...
f_fun, ...
g_fun, ...
h_fun, ...
max_itr, ...
epsilon, ...
x_in, ...
z, ...
qinv, ...
rinv, ...
level ...
);
% ----------------------------------------------------------------------
ok = ok & (size(info,1) <= max_itr);
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] = nonlinear_ok_sin_f(k, xk);
[gk, Gk] = nonlinear_ok_box_g(k, xk1);
[hk, Hk] = nonlinear_ok_box_h(k, xk);
%
ok = ok & all( fk <= epsilon );
ok = ok & all( abs(fk) .* uk <= epsilon );
%
df_out(:,:, k) = Fk;
dg_out(:,:, k) = Gk;
dh_out(:,:, k) = Hk;
f_out(:, k) = fk - Fk * xk;
g_out(:, k) = gk - Gk * xk1;
h_out(:, k) = hk - Hk * xk;
xk1 = xk;
end
ok = ok & all( all( u_out >= 0 ) );
%
d_out = ckbs_sumsq_grad(x_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 ) ) <= epsilon);
end
if ~ ok
keyboard
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-' );
plot(x_true(2,:)', 1 - sin(x_true(2,:))' + x4_min, 'r-');
axis([h_min, h_max, v_min, v_max]);
title('Constrained: blue=truth, green=estimate, red=constraint');
hold off
%
% constrained estimate
x_con = x_out;
end
% ----------------------------------------------------------------------
% Unconstrained case
f_fun = 'nonlinear_ok_box_nof'; % same function as used by nonlinear_ok_box
[x_out, u_out, info] = ckbs_nonlinear( ...
f_fun, ...
g_fun, ...
h_fun, ...
max_itr, ...
epsilon, ...
x_in, ...
z, ...
qinv, ...
rinv, ...
level ...
);
% ----------------------------------------------------------------------
ok = ok & (size(info,1) <= max_itr);
xk1 = zeros(n, 1);
for k = 1 : N
xk = x_out(:, k);
[gk, Gk] = nonlinear_ok_box_g(k, xk1);
[hk, Hk] = nonlinear_ok_box_h(k, xk);
%
dg_out(:,:, k) = Gk;
dh_out(:,:, k) = Hk;
g_out(:, k) = gk - Gk * xk1;
h_out(:, k) = hk - Hk * xk;
xk1 = xk;
end
d_out = ckbs_sumsq_grad(x_out, z, g_out, h_out, dg_out, dh_out, qinv, rinv);
ok = ok & (max( max( abs(d_out) ) ) <= epsilon);
if draw_plot
figure(2);
clf
hold on
plot(x_true(2,:)', x_true(4,:)', 'b-' );
plot(x_out(2,:)', x_out(4,:)', 'g-' );
plot(x_true(2,:)', 1 - sin(x_true(2,:))' + x4_min, 'r-');
axis([h_min, h_max, v_min, v_max]);
title('Unconstrained: blue=truth, green=estimate, red=constraint');
hold off
%
% unconstrained estimate
x_free = x_out;
%
% write out constrained and unconstrained results
[fid, msg] = fopen('nonlinear_ok_sin.out', 'wt');
if size(msg, 2) > 0
disp(['nonlinear_ok: ', msg]);
end
% 123456789012345678901234'
heading = ' x2_true x2_con x2_free' ;
heading = [ heading , ' x4_true x4_con x4_free' ];
heading = [ heading , ' z1 z2\n' ];
fprintf(fid, heading);
for k = 1 : N
fprintf(fid,'%8.3f%8.3f%8.3f%8.3f%8.3f%8.3f%8.3f%8.3f\n', ...
x_true(2,k), x_con(2,k), x_free(2,k), ...
x_true(4,k), x_con(4,k), x_free(4,k), ...
z(1,k), z(2,k) ...
);
end
fclose(fid);
end
return
end