source: projects/ckbs/trunk/test/nonlinear_ok_sin.m @ 95

Last change on this file since 95 was 35, checked in by bradbell, 11 years ago

Revmove reference to gpl2.txt and further shorten copyright and license statement

File size: 8.5 KB
Line 
1% -------------------------------------------------------------------
2% ckbs: Constrained Kalman-Bucy Smoother Program: Copyright (C) 2006
3% Authors: Bradlely Bell:        bradbell at washington dot edu
4%          Gianluigi Pillonetto: giapi at dei dot unipd dot it
5% License: GNU General Public License Version 2
6% -------------------------------------------------------------------
7% $begin nonlinear_ok_sin.m$$ $newlinech %$$
8% $spell
9%       itr
10%       dist
11%       ckbs
12%       dt
13%       nargin
14%       cos
15%       randn
16%       rinv
17%       rinvk
18%       qk
19%       qinv
20%       qinvk
21%       inv
22%       xk
23%       Fk
24%       Gk
25%       Hk
26%       disp
27%       fprintf
28%       fid
29%       fclose
30%       df
31%       dg
32%       dh
33%       uk
34%       sumsq
35%       clf
36%       nof
37%       wt
38%       dk
39%       fopen
40% $$
41%
42% $section ckbs_nonlinear Sine Wave Constrained Tracking Example and Test$$
43%
44% $index tracking, example and test$$
45% $index example, tracking$$
46% $index test, tracking$$
47% $index sine, constraint example$$
48%
49% $index ckbs, example and test$$
50% $index example, ckbs_nonlinear$$
51% $index test, ckbs_nonlinear$$
52%
53% $head Syntax$$
54% $syntax%[%ok%] = nonlinear_ok_sin(%draw_plot%)%$$
55%
56% $head draw_plot$$
57% If this argument is true, a plot is drawn showing the results
58% and the $code nonlinear_ok_sin.out$$ file is written for use with the
59% program $code test/nonlinear_ok_sin.r$$.
60%
61% $head ok$$
62% If the return value $italic ok$$ is true, the test passed,
63% otherwise the test failed.
64%
65% $head State Vector$$
66% $table
67% $latex x1 (t)$$  $cnext first component of velocity   $rnext
68% $latex x2 (t)$$  $cnext first component of position  $rnext
69% $latex x3 (t)$$  $cnext second component of velocity   $rnext
70% $latex x4 (t)$$  $cnext second component of position 
71% $tend
72%
73% $head Measurement Vector$$
74% $table
75% $latex z1 (t)$$  $cnext range to station s1 plus noise $rnext
76% $latex z2 (t)$$  $cnext range to station s2  plus noise
77% $tend
78%
79% $head Constraint$$
80% $latex  x4(t) \geq 1 - sin( x2(t) )$$
81%
82% $children%
83%       test/nonlinear_ok_sin_f.m
84% %$$
85% $head Call Back Functions$$
86% $table
87% $rref nonlinear_ok_sin_f.m$$
88% $rref nonlinear_ok_box_g.m$$
89% $rref nonlinear_ok_box_h.m$$
90% $rref nonlinear_ok_box_nof.m$$
91% $tend
92%
93% $head Source Code$$
94% $newlinech $$ $codep
95function [ok] = nonlinear_ok_sin(draw_plot)
96if nargin < 1
97        draw_plot = false;
98end
99% --------------------------------------------------------
100% You can change these parameters
101N       = 50;            % number of measurement time points
102dt      = 2 * pi / N;    % time between measurement points
103sigma   = .25;           % standard deviation of measurement noise
104gamma   = 1.;            % multiplier for transition variance
105max_itr = 50;            % maximum number of iterations
106epsilon = 1e-4;          % convergence criteria
107x4_min  = .25;           % minimum value for x4 constraint
108h_min   = 0;             % minimum horizontal value in plots
109h_max   = 7;             % maximum horizontal value in plots
110v_min   = 0;             % minimum vertical value in plots
111v_max   = 2.5;           % maximum vertical value in plots
112x4_dist = .05;           % distance true path is from the constraint
113s1      = [ 0 , 0 ];     % station one
114s2      = [ 2*pi , 0 ];  % station two
115%
116% level of tracing during the optimization
117if draw_plot
118        level   = 1;
119else
120        level   = 0;
121end
122% ---------------------------------------------------------
123% global variables used by nonlinear_ok_box_h
124global nonlinear_ok_box_s1
125global nonlinear_ok_box_s2
126nonlinear_ok_box_s1 = s1;
127nonlinear_ok_box_s2 = s2;
128%
129% global variable used by nonlinear_ok_sin_f
130global nonlinear_ok_sin_x4_min
131nonlinear_ok_sin_x4_min = x4_min;
132%
133ok = true;
134%
135%  Define the problem
136rand('seed', 234);
137%
138% number of constraints per time point
139ell   = 1;
140%
141% number of measurements per time point
142m     = 2;
143%
144% number of state vector components per time point
145n     = 4;
146%
147% simulate the true trajectory and measurement noise
148t        =  (1 : N) * dt;
149x1_true  = ones(1, N);
150x2_true  = t;
151x3_true  = - cos(t);
152x4_true  = x4_dist + 1 - sin(t) + x4_min;
153x_true   = [ x1_true ; x2_true ; x3_true ; x4_true ];
154v1_true  = sigma * randn(1, N);
155v2_true  = sigma * randn(1, N);
156%
157% corresponding measurement values
158rinv    = zeros(m, m, N);
159z       = zeros(m, N);
160rinvk   = eye(m) / (sigma * sigma);
161for k = 1 : N
162        x_k          = x_true(:, k);
163        h_k          = nonlinear_ok_box_h(k, x_k);
164        z(:, k)      = h_k + [ v1_true(k) ; v2_true(k) ];
165        rinv(:,:, k) = rinvk;
166end
167%
168% covariance for the transition noise
169qk       = diag( [ dt, dt^3 / 3 , dt, dt^3 / 3 ] );
170qk(1, 2) = dt^2 / 2;
171qk(2, 1) = dt^2 / 2;
172qk(3, 4) = dt^2 / 2;
173qk(4, 3) = dt^2 / 2;
174qk       = qk * gamma;
175qinvk    = inv(qk);
176qinv     = zeros(n, n, N);
177for k = 2 : N
178        qinv(:,:, k) = qinvk;
179end
180%
181% covariance for the initial estimate
182qinv(:,:,1) = eye(n) * 100 * gamma;
183%
184% initial x vector
185x_in       = zeros(n, N);
186x_in(4, :) = 1;
187%
188% nonlinear_ok_sin use same g function as nonlinear_box_ok
189% global variables used by nonlinear_ok_box_g
190global nonlinear_ok_box_x1
191global nonlinear_ok_box_dt
192nonlinear_ok_box_x1 = x_true(:, 1);
193nonlinear_ok_box_dt = dt;
194%
195% ----------------------------------------------------------------------
196f_fun = 'nonlinear_ok_sin_f';  % special nonlinear_ok_sin constraints
197g_fun = 'nonlinear_ok_box_g';  % same transition function as nonlinear_ok_box
198h_fun = 'nonlinear_ok_box_h';  % same measurement function as nonlinear_ok_box
199[x_out, u_out, info] = ckbs_nonlinear( ...
200        f_fun,    ...
201        g_fun,    ...
202        h_fun,    ...
203        max_itr,  ...
204        epsilon,  ...
205        x_in,     ...
206        z,        ...
207        qinv,     ...
208        rinv,     ...
209        level     ...
210);
211% ----------------------------------------------------------------------
212ok         = ok & (size(info,1) <= max_itr);
213f_out      = zeros(ell, N);
214g_out      = zeros(n, N);
215h_out      = zeros(m, N);
216df_out     = zeros(ell, n, N);
217dg_out     = zeros(n, n, N);
218dh_out     = zeros(m, n, N);
219xk1        = zeros(n, 1);
220for k = 1 : N
221        xk    = x_out(:, k);
222        uk    = u_out(:, k);
223        [fk, Fk]   = nonlinear_ok_sin_f(k, xk);
224        [gk, Gk]   = nonlinear_ok_box_g(k, xk1);
225        [hk, Hk]   = nonlinear_ok_box_h(k, xk);
226        %
227        ok   = ok & all( fk <= epsilon );
228        ok   = ok & all( abs(fk) .* uk <= epsilon );
229        %
230        df_out(:,:, k) = Fk;
231        dg_out(:,:, k) = Gk;
232        dh_out(:,:, k) = Hk;
233        f_out(:, k)    = fk - Fk * xk;
234        g_out(:, k)    = gk - Gk * xk1;
235        h_out(:, k)    = hk - Hk * xk;
236        xk1 = xk;
237end
238ok   = ok & all( all( u_out >= 0 ) );
239%
240d_out = ckbs_sumsq_grad(x_out, z, g_out, h_out, dg_out, dh_out, qinv, rinv);
241for k = 1 : N
242        uk = u_out(:, k);
243        Fk = df_out(:,:, k);
244        dk = d_out(:, k);
245        %
246        ok = ok & (max ( abs( Fk' * uk + dk ) ) <= epsilon);
247end
248if ~ ok
249        keyboard
250end
251if draw_plot
252        figure(1);
253        clf
254        hold on
255        plot(x_true(2,:)', x_true(4,:)', 'b-' );
256        plot(x_out(2,:)', x_out(4,:)', 'g-' );
257        plot(x_true(2,:)', 1 - sin(x_true(2,:))' + x4_min, 'r-');
258        axis([h_min, h_max, v_min, v_max]);
259        title('Constrained: blue=truth, green=estimate, red=constraint');
260        hold off
261        %
262        % constrained estimate
263        x_con = x_out;
264end
265% ----------------------------------------------------------------------
266% Unconstrained case
267f_fun = 'nonlinear_ok_box_nof';  % same function as used by nonlinear_ok_box
268[x_out, u_out, info] = ckbs_nonlinear( ...
269        f_fun,    ...
270        g_fun,    ...
271        h_fun,    ...
272        max_itr,  ...
273        epsilon,  ...
274        x_in,     ...
275        z,        ...
276        qinv,     ...
277        rinv,     ...
278        level     ...
279);
280% ----------------------------------------------------------------------
281ok   = ok & (size(info,1) <= max_itr);
282xk1  = zeros(n, 1);
283for k = 1 : N
284        xk    = x_out(:, k);
285        [gk, Gk]   = nonlinear_ok_box_g(k, xk1);
286        [hk, Hk]   = nonlinear_ok_box_h(k, xk);
287        %
288        dg_out(:,:, k) = Gk;
289        dh_out(:,:, k) = Hk;
290        g_out(:, k)    = gk - Gk * xk1;
291        h_out(:, k)    = hk - Hk * xk;
292        xk1 = xk;
293end
294d_out  = ckbs_sumsq_grad(x_out, z, g_out, h_out, dg_out, dh_out, qinv, rinv);
295ok     = ok & (max( max( abs(d_out) ) ) <= epsilon);
296if draw_plot
297        figure(2);
298        clf
299        hold on
300        plot(x_true(2,:)', x_true(4,:)', 'b-' );
301        plot(x_out(2,:)', x_out(4,:)', 'g-' );
302        plot(x_true(2,:)', 1 - sin(x_true(2,:))' + x4_min, 'r-');
303        axis([h_min, h_max, v_min, v_max]);
304        title('Unconstrained: blue=truth, green=estimate, red=constraint');
305        hold off
306        %
307        % unconstrained estimate
308        x_free = x_out;
309        %
310        % write out constrained and unconstrained results
311        [fid, msg] = fopen('nonlinear_ok_sin.out', 'wt');
312        if size(msg, 2) > 0
313                disp(['nonlinear_ok: ', msg]);
314        end
315        %                      123456789012345678901234'
316        heading =             ' x2_true  x2_con x2_free'  ;
317        heading = [ heading , ' x4_true  x4_con x4_free' ];
318        heading = [ heading , '      z1      z2\n'       ];
319        fprintf(fid, heading);
320        for k = 1 : N
321                fprintf(fid,'%8.3f%8.3f%8.3f%8.3f%8.3f%8.3f%8.3f%8.3f\n', ...
322                        x_true(2,k), x_con(2,k), x_free(2,k), ...
323                        x_true(4,k), x_con(4,k), x_free(4,k), ...
324                        z(1,k), z(2,k) ...
325                );
326        end
327        fclose(fid);
328end
329return
330end
331% $$ $newlinech %$$
332% $end
Note: See TracBrowser for help on using the repository browser.