function [ok] = nonlinear_ok_simple()
% --------------------------------------------------------------------
max_itr = 20; % maximum number of iterations
epsilon = 1e-5; % convergence criteria
N = 3; % number of time points
ell = 2; % number of constraints
m = 1; % number of measurements per time point
n = 3; % number of state vector components per time point
sigma = 1; % variance of measurement noise
level = 0; % level of tracing in ckbs_nonlinear
%
% simulated true trajectory
x_true = ones(n, 1) * (1 : N);
%
% simulate measurement noise
e_true = zeros(m, N);
%
% initial x vector
x_in = zeros(n, N);
%
% --------------------------------------------------------------------
if m > n
error('nonlinear_ok_simple: m > n');
end
if ell > n
error('nonlinear_ok_simple: ell > n');
end
%
global nonlinear_ok_simple_m
global nonlinear_ok_simple_ell
global nonlinear_ok_simple_N
%
nonlinear_ok_simple_m = m;
nonlinear_ok_simple_ell = ell;
nonlinear_ok_simple_N = N;
%
% measurement variances
rinv = zeros(m, m, N);
qinv = zeros(n, n, N);
qinvk = eye(n) / (sigma * sigma);
rinvk = eye(m) / (sigma * sigma);
z = zeros(m, N);
for k = 1 : N
xk = x_true(:, k);
ek = e_true(:, k);
hk = nonlinear_ok_simple_h(k, xk);
z(:, k) = hk + ek;
qinv(:,:, k) = qinvk;
rinv(:,:, k) = rinvk;
end
%
% pass the maximum state value to f
global nonlinear_ok_simple_f_max
nonlinear_ok_simple_f_max = (N - .5) * ones(m, 1);
%
% ----------------------------------------------------------------------
f_fun = 'nonlinear_ok_simple_f';
g_fun = 'nonlinear_ok_simple_g';
h_fun = 'nonlinear_ok_simple_h';
[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;
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] = feval(f_fun, k, xk);
[gk, Gk] = feval(g_fun, k, xk1);
[hk, Hk] = feval(h_fun, k, xk);
%
ok = ok & all( fk <= epsilon);
ok = ok & all( abs(fk) .* uk <= epsilon );
%
f_out(:, k) = fk - Fk * xk;
g_out(:, k) = gk - Gk * xk1;
h_out(:, k) = hk - Hk * xk;
df_out(:,:, k) = Fk;
dg_out(:,:, k) = Gk;
dh_out(:,:, k) = Hk;
xk1 = xk;
end
ok = ok & all( all( u_out >= 0 ) );
grad = 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 = grad(:, k);
%
ok = ok & all( abs(Fk' * uk + dk) <= epsilon );
end
return
end