Previous Next

Simple ckbs_nonlinear Example and Test

Call Back Functions
nonlinear_ok_simple_f.m Example Constraint Function: nonlinear_ok_simple_f
nonlinear_ok_simple_g.m Example Transition Function: nonlinear_ok_simple_g
nonlinear_ok_simple_h.m Example Measurement Function: nonlinear_ok_simple_h

Source Code
 
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

Input File: test/nonlinear_ok_simple.m