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