Previous Next

ckbs_nonlinear Sine Wave Constrained Tracking Example and Test

Syntax
[ok] = nonlinear_ok_sin(draw_plot)

draw_plot
If this argument is true, a plot is drawn showing the results and the nonlinear_ok_sin.out file is written for use with the program test/nonlinear_ok_sin.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 station s1 plus noise
 z2 (t) range to station s2 plus noise

Constraint
  x4(t) \geq 1 - sin( x2(t) )

Call Back Functions
nonlinear_ok_sin_f.m Example Constraint Function: nonlinear_ok_sin_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_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

Input File: example/nonlinear_ok_sin.m