Previous Next sine_wave_ok.m

ckbs_nonlinear: Example and Test of Tracking a Sine Wave

Syntax
[ok] = sine_wave_ok(constraintdraw_plot)

constraint
is a character row vector with one of the following values:
value meaning
no_constraint solve unconstrained problem
box_constraint use box constraints
sine_constraint use a sine wave constraint

draw_plot
If draw_plot is false, no plot is drawn. If this argument is true, a plot is drawn showing the results.

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

Simulated State Vector
 x1 (t) = 1 first component of velocity
 x2 (t) = t first component of position
 x3 (t) = \cos(t) second component of velocity
 x4 (t) = \sin(t) second component of position

Transition Model
We simulate imperfect knowledge of the dynamical system as follows  \[
\begin{array}{rcl}
     x_{1,k} & = & x_{1,k-1} + x_{2,k-1} \Delta t + w_{1,k}
     \\
     x_{2,k} & = & x_{2,k-1} + w_{2,k}
     \\
     x_{3,k} & = & x_{3,k-1} + x_{4,k-1} \Delta t + w_{3,k}
     \\
     x_{4,k} & = & x_{4,k-1} + w_{4,k}
\end{array}
\] 
where  \Delta t = 2 \pi / N is the time between measurements,  N = 50 is the number of measurement times,  w_k \sim \B{N}( 0 , Q_k ) , and the transition variance is given by  \[
Q_k = \left( \begin{array}{cccc}
     \Delta t       & \Delta t^2 / 2 & 0 & 0 \\
     \Delta t^2 / 2 & \Delta t^3 / 3 & 0 & 0 \\
     0              & 0              & \Delta t       & \Delta t^2 / 2 \\
     0              & 0              & \Delta t^2 / 2 & \Delta t^3 / 3
\end{array} \right)
\] 
The routine pos_vel_g.m calculates the mean for  x_k given  x_{k-1} .

Initial State Estimate
The routine pos_vel_g.m also returns the initial state estimate  \hat{x} = g_1 ( x_0 ) as the simulated state value at the first measurement point; i.e.,  \[
     \hat{x} = [ 1 , \Delta t , \cos ( \Delta t ) , \sin ( \Delta t ) ]^\R{T}
\] 
The variance of the initial state estimate is  Q_1 = 10^4 I_4  where  I_4 is the four by four identity matrix.

Measurement Model
For this example, the measurements are noisy observations of the distance to two positions  a = (0, -1.5)^\R{T} and  b = ( 2 \pi , -1.5) ; i.e.,  \[
\begin{array}{rcl}
     z_{1,k} & = & \sqrt{ ( x_{2,k} - a_1 )^2 + ( x_{4,k} - a_2 )^2 } + v_{1,k}
     \\
     z_{2,k} & = & \sqrt{ ( x_{2,k} - b_1 )^2 + ( x_{4,k} - b_2 )^2 } + v_{2,k}
\end{array}
\] 
where  v_k \sim \B{N} ( 0 , R_k ) and the measurement variance  R_k = \R{diag}( .25 , .25 ) . The routine distance_h.m calculates the mean for  z_k , given the value of  x_k .

Exception
The simulated state sequence and the measurement model above are used to simulate the data with the following exception: if  \hat{k} is the time index where  x_{4,k} is maximal,  \[
\begin{array}{rcl}
     z_{1,k} & = & 
     \sqrt{ ( x_{2,k} - a_1 )^2 + ( x_{4,k} + .5 - a_2 )^2 } 
     \\
     z_{2,k} & = & 
     \sqrt{ ( x_{2,k} - b_1 )^2 + ( x_{4,k} + .5 - b_2 )^2 }
\end{array}
\] 
This increases the probability that the upper constraint will be active at the optimal solution.

Constraint Model
The are three possible constraints depending on the constraint argument:

no_constraint
if constraint == 'no_constraint' , the function no_f.m is used and the unconstrained problem is solved.

box_constraint
if constraint == 'box_constraint' , the function box_f.m is used to represent the following constraints: for  k = 1 , \ldots , N ,  \[
     -1 \leq x_{4,k} \leq +1
\] 


sine_constraint
if constraint == 'sine_constraint' , the function sine_f.m is used to represent the following constraints: for  k = 1 , \ldots , N ,  \[
     x_{4,k} \leq \sin( x_{2,k} ) + .1
\] 


Call Back Functions
pos_vel_g.m ckbs_nonlinear: Example Position and Velocity Transition Model
distance_h.m ckbs_nonlinear: Example of Distance Measurement Model
no_f.m ckbs_nonlinear: Example of No Constraint
box_f.m ckbs_nonlinear: Example of Box Constraints
sine_f.m ckbs_nonlinear: Example of Nonlinear Constraint

Source Code
 
function [ok] = sine_wave_ok(constraint, draw_plot)
% --------------------------------------------------------
% Simulation problem parameters
max_itr    = 25;             % maximum number of iterations
epsilon    = 1e-4;           % convergence criteria
N          = 50;             % number of measurement time points
dt         = 2 * pi / N;     % time between measurement points
sigma_r    = .5;             % standard deviation of measurement noise
sigma_q    = 1;              % square root of multiplier for transition variance
sigma_e    = 100;            % standard deviation           
delta_box  = 0.;             % distance from truth to box constraint
delta_sine = .1;             % distance from truth to sine constraint
h_min      = 0;              % minimum horizontal value in plots
h_max      = 7;              % maximum horizontal value in plots
v_min      = -1.5;           % minimum vertical value in plots
v_max      = +1.5;           % maximum vertical value in plots 
a          = [ 0    ; -1.5]; % position that range one measures from
b          = [ 2*pi ; -1.5]; % position that range two measures to
randn('seed', 1234);         % Random number generator seed
% ---------------------------------------------------------
% level of tracing during the optimization
if draw_plot
	level   = 1;
else
	level   = 0;
end
% ---------------------------------------------------------
% simulate the true trajectory and measurement noise
t        =  (1 : N) * dt;         % time
x_true   = [ ones(1, N) ; t ; cos(t) ; sin(t) ];
initial  = x_true(:,1);
% ---------------------------------------------------------
% information used by box_f
global box_f_info
box_f_info.lower = - 1 - delta_box;
box_f_info.upper = + 1 + delta_box;
box_f_info.index = 4;
% ---------------------------------------------------------
% information used by sine_f
global sine_f_info
sine_f_info.index  = [ 2 ; 4 ];
sine_f_info.offset = [ 0 ; delta_sine ];
% ---------------------------------------------------------
% information used by pos_vel_g
global pos_vel_g_info
pos_vel_g_info.dt       = dt;
pos_vel_g_info.initial  = initial;
% ---------------------------------------------------------
% global information used by distance_h
global distance_h_info
distance_h_info.index    = [ 2 ; 4 ];
distance_h_info.position = [a , b ];
% ---------------------------------------------------------
% problem dimensions
m     = 2; % number of measurements per time point
n     = 4; % number of state vector components per time point
ell   = 0; % number of constraint (reset to proper value below)
% ---------------------------------------------------------
% simulate the measurements
[c,k_max] = max(x_true(4,:));    % index where second position component is max
rinv      = zeros(m, m, N);      % inverse covariance of measurement noise
z         = zeros(m, N);         % measurement values
for k = 1 : N
	x_k   = x_true(:, k);
	if k == k_max
		% special case to increase chance that upper constraint is active 
		x_k(2)   = x_k(2) + .5;
		h_k      = distance_h(k, x_k);
		z(:, k)  = h_k; 
	else
		% simulation according to measurement model
		h_k      = distance_h(k, x_k);
		z(:, k)  = h_k + sigma_r * randn(2, 1); 
	end
	rinv(:,:, k) = eye(m) / (sigma_r * sigma_r);
end
% ---------------------------------------------------------
% inverse transition variance
qk       = sigma_q * sigma_q * [ ...
	dt       , dt^2 / 2 , 0 , 0 
	dt^2 / 2 , dt^3 / 3 , 0 , 0 
	0        , 0        , dt       , dt^2 / 2
	0        , 0        , dt^2 / 2 , dt^3 / 3
];
qinvk    = inv(qk);
qinv     = zeros(n, n, N);
for k = 2 : N
	qinv(:,:, k) = qinvk;
end
%
% inverse initial estimate covariance
qinv(:,:,1) = eye(n) * sigma_e * sigma_e; 
% ----------------------------------------------------------
% initialize x vector at position and velocity zero
x_in = zeros(n, N);
% ----------------------------------------------------------------------
g_fun     = @ pos_vel_g;
h_fun     = @ distance_h;
%
is_no_f   = false;
is_box_f  = false;
is_sine_f = false;
switch constraint
	case {'no_constraint'}
	ell       = 1;
	f_fun     = @ no_f;
	is_no_f   = true;

	case {'box_constraint'}
	ell       = 2;
	f_fun     = @ box_f;
	is_box_f  = true;

	case {'sine_constraint'}
 	ell       = 1;
 	f_fun     = @ sine_f;
	is_sine_f = true;

	otherwise
	constraint = constraint
	error('sine_wave_ok: invalid value for constraint')
end
% ----------------------------------------------------------------------
[x_out, u_out, info] = ckbs_nonlinear( ...
	f_fun,    ...
	g_fun,    ...
	h_fun,    ...
	max_itr,  ...
	epsilon,  ...
	x_in,     ...
	z,        ...
	qinv,     ...
	rinv,     ...
	level     ...
);
% ----------------------------------------------------------------------
ok         = true;
ok         = ok & (size(info,1) <= max_itr);
%
% Set up affine problem (for change in x) corresponding to x_out.
% Check constraint feasibility and complementarity.
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]   = f_fun(k, xk);
	[gk, Gk]   = pos_vel_g(k, xk1);
	[hk, Hk]   = distance_h(k, xk);
	%
	ok   = ok & all( fk <= epsilon );              % feasibility
	ok   = ok & all( abs(fk) .* uk <= epsilon );   % complementarity
	%
	f_out(:, k)    = fk;
	g_out(:, k)    = gk - xk;
	h_out(:, k)    = hk;
	df_out(:,:, k) = Fk;
	dg_out(:,:, k) = Gk;
	dh_out(:,:, k) = Hk;
	%
	xk1 = xk;
end
ok   = ok & all( all( u_out >= 0 ) );
%
% Compute gradient of unconstrained affine problem at zero
% and check gradient of Lagrangian
dx_out = zeros(n, N);
d_out  = ckbs_sumsq_grad(dx_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 draw_plot
	figure(1)
	clf
	hold on
	plot( x_true(2,:) , x_true(4,:) , 'b-' );
	plot( x_out(2,:)  , x_out(4,:)  , 'g-' );
	if is_no_f
		title('blue=truth, green=estimate, no constraint')
	end
	if is_box_f
		title('blue=truth, green=estimate, red=linear constraint')
		plot( t , + ones(1, N) + delta_box , 'r-'); 
		plot( t , - ones(1, N) - delta_box , 'r-'); 
	end
	if is_sine_f
		title('blue=truth, green=estimate, red=nonlinear constraint')
		plot( x_out(2,:) , sin( x_out(2,:) ) + delta_sine , 'r-'); 
	end
	axis( [ h_min, h_max, v_min, v_max] );
return
end

Input File: example/nonlinear/sine_wave_ok.m