Previous Next

ckbs_nonlinear Box Constrained Tracking Example and Test

Syntax
[ok] = nonlinear_ok_box(draw_plot)

draw_plot
If this argument is true, a plot is drawn showing the results and the nonlinear_ok_box.out file is written for use with the program nonlinear_ok_box.r.

ok
If the return value ok is true, the test passed, otherwise the test failed.

State Vector
 x1 (t) first component of velocity
 x2 (t) first component of position
 x3 (t) second component of velocity
 x4 (t) second component of position

Measurement Vector
 z1 (t) range to the station s1 plus noise
 z2 (t) range to the station s2 plus noise

Constraint
 2 + x4\_min \geq x_4 (t) \geq x4\_min  .

Call Back Functions
nonlinear_ok_box_f.m Example Constraint Function: nonlinear_ok_box_f
nonlinear_ok_box_g.m Example Transition Function: nonlinear_ok_box_g
nonlinear_ok_box_h.m Example Measurement Function: nonlinear_ok_box_h
nonlinear_ok_box_nof.m Example no Constraint Function: nonlinear_ok_box_nof

Source Code
 
function [ok] = nonlinear_ok_box(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_true
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 
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;
%
ok = true;
%
%  Define the problem
rand('seed', 1234);
%
% number of constraints per time point
ell   = 2;
%
% 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;
x2_true  = t; 
x4_max   = x4_min + 2;
x4_true  = x4_min + 1 - sin(t);
x1_true  = ones(1, N);
x3_true  = - cos(t);
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);
for k = 1 : N
	x_in(:, k) = [ 0 ; 1 ; 0 ; 1 ];
end
%
% global variables used by nonlinear_ok_box_f
global nonlinear_ok_box_x4_min
global nonlinear_ok_box_x4_max
nonlinear_ok_box_x4_min = x4_min;
nonlinear_ok_box_x4_max = x4_max;
%
% 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_box_f';
g_fun = 'nonlinear_ok_box_g';
h_fun = 'nonlinear_ok_box_h';
[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_box_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,:)', x4_min * ones(N,1), 'r-');
	plot(x_true(2,:)', x4_max * ones(N,1), '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';
[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,:)', x4_min * ones(1,N), 'r-');
	plot(x_true(2,:)', x4_max * ones(1,N), '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_box.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

Input File: example/nonlinear_ok_box.m