source: projects/ckbs/trunk/test/affine_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: 6.0 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 affine_ok_box.m$$ $newlinech %$$
8% $spell
9%       itr
10%       ko
11%       nargin
12%       clf
13%       fid
14%       fopen
15%       wt
16%       fprintf
17%       fclose
18%       disp
19%       diff
20%       ckbs
21%       bk
22%       cos
23%       dg
24%       dh
25%       dk
26%       dt
27%       inv
28%       qinv
29%       qinvk
30%       qk
31%       randn
32%       rinv
33%       rinvk
34%       rk
35%       sk
36%       sumsq
37%       uk
38%       xk
39% $$
40%
41% $section ckbs_affine Box Constrained Smoothing Spline Example and Test$$
42%
43% $index smoothing, spline example and test$$
44% $index example, smoother spline$$
45% $index test, smoothing spline$$
46%
47% $index ckbs_affine, example and test$$
48% $index affine, example and test$$
49% $index example, affine$$
50% $index test, affine$$
51%
52% $head Syntax$$
53% $syntax%[%ok%] = affine_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 derivative of function we are estimating $rnext
67% $latex x2 (t)$$ $cnext value of function we are estimating
68% $tend
69%
70% $head Measurement$$
71% $latex z1 (t)$$ value of $latex x2 (t)$$ plus noise
72%
73% $head Constraint$$
74% $latex \[
75%       \begin{array}{c}
76%       -1 \leq x1 (t) \leq +1
77%       \\
78%       -1 \leq x2 (t) \leq +1
79%       \end{array}
80% \]$$.
81%
82% $head Source Code$$
83%
84% $newlinech $$ $codep
85function [ok] = affine_ok_box(draw_plot)
86% --------------------------------------------------------
87% You can change these parameters
88N     = 50;        % number of measurement time points
89dt    = 2*pi / N;  % time between measurement points
90gamma =  1;        % transition covariance multiplier
91sigma =  .5;       % standard deviation of measurement noise
92max_itr = 30;      % maximum number of iterations
93epsilon = 1e-5;    % convergence criteria
94h_min   = 0;       % minimum horizontal value in plots
95h_max   = 7;       % maximum horizontal value in plots
96v_min   = -2.0;    % minimum vertical value in plots
97v_max   = +2.0;    % maximum vertical value in plots
98% ---------------------------------------------------------
99ok = true;
100if nargin < 1
101        draw_plot = false;
102end
103%  Define the problem
104rand('seed', 1234);
105%
106% number of constraints per time point
107ell   = 4;
108%
109% number of measurements per time point
110m     = 1;
111%
112% number of state vector components per time point
113n     = 2;
114%
115% simulate the true trajectory and measurement noise
116t       =  (1 : N) * dt;
117x1_true = - cos(t);
118x2_true = - sin(t);
119x_true  = [ x1_true ; x2_true ];
120v_true  = sigma * rand(1, N);
121%
122% measurement values and model
123v_true  = sigma * randn(1, N);
124z       = x2_true + v_true;
125rk      = sigma * sigma;
126rinvk   = 1 / rk;
127rinv    = zeros(m, m, N);
128h       = zeros(m, N);
129dh      = zeros(m, n, N);
130for k = 1 : N
131        rinv(:, :, k) = rinvk;
132        h(:, k)       = 0;
133        dh(:,:, k)    = [ 0 , 1 ];
134end
135%
136% transition model
137g       = zeros(n, N);
138dg      = zeros(n, n, N);
139qinv    = zeros(n, n, N);
140qk      = gamma * [ dt , dt^2/2 ; dt^2/2 , dt^3/3 ];
141qinvk   = inv(qk);
142for k = 2 : N
143        g(:, k)       = 0;
144        dg(:,:, k)    = [ 1 , 0 ; dt , 1 ];
145        qinv(:,:, k)  = qinvk;
146end
147%
148% initial state estimate
149g(:, 1)      = x_true(:, 1);
150qinv(:,:, 1) = 100 * eye(2);
151%
152% constraints
153b       = zeros(ell, N);
154db      = zeros(ell, n, N);
155for k = 1 : N
156        %
157        % b(:, k) + db(:,:, k) * x(:, k) <= 0
158        b(:, k)    = [ -1 ; -1 ; -1 ; -1 ];
159        db(:,:, k) = [ -1 , 0 ; 1 , 0 ; 0 , -1 ; 0 , 1 ];
160end
161%
162% -------------------------------------------------------------------------
163[xOut, uOut, info] = ...
164        ckbs_affine(max_itr, epsilon, z, b, g, h, db, dg, dh, qinv, rinv);
165% --------------------------------------------------------------------------
166ok   = ok & all( info(end, 1:3) <= epsilon);
167d    = ckbs_sumsq_grad(xOut, z, g, h, dg, dh, qinv, rinv);
168for k = 1 : N
169        xk = xOut(:, k);
170        uk = uOut(:, k);
171        bk = b(:, k);
172        Bk = db(:,:, k);
173        dk = d(:, k);
174        sk = - bk - Bk * xk;
175        %
176        ok = ok & (min(uk) >= 0.);
177        ok = ok & (max (bk + Bk * xk) <= epsilon);
178        ok = ok & (max ( abs( Bk' * uk + dk ) ) <= epsilon);
179        ok = ok & (max ( uk .* sk ) <= epsilon );
180end
181if draw_plot
182        figure(1);
183        clf
184        hold on
185        plot(t', x_true(2,:)', 'r-' );
186        plot(t', z(1,:)', 'ko' );
187        plot(t', xOut(2,:)', 'b-' );
188        plot(t', - ones(N,1), 'b-');
189        plot(t', ones(N,1), 'b-');
190        axis([h_min, h_max, v_min, v_max]);
191        title('Constrained');
192        hold off
193        %
194        % constrained estimate
195        x_con = xOut;
196end
197%
198% Unconstrained Case
199b           = zeros(0, N);
200db          = zeros(0, n, N);
201% -------------------------------------------------------------------------
202[xOut, uOut, info] = ...
203        ckbs_affine(max_itr, epsilon, z, b, g, h, db, dg, dh, qinv, rinv);
204% --------------------------------------------------------------------------
205ok   = ok & all( info(end, 1:3) <= epsilon);
206d    = ckbs_sumsq_grad(xOut, z, g, h, dg, dh, qinv, rinv);
207for k = 1 : N
208        xk = xOut(:, k);
209        dk = d(:, k);
210        %
211        ok = ok & (min(dk) <= epsilon);
212end
213if draw_plot
214        figure(2);
215        clf
216        hold on
217        plot(t', x_true(2,:)', 'r-' );
218        plot(t', z(1,:)', 'ko' );
219        plot(t', xOut(2,:)', 'b-' );
220        plot(t', - ones(N,1), 'b-');
221        plot(t', ones(N,1), 'b-');
222        axis([h_min, h_max, v_min, v_max]);
223        title('Unconstrained');
224        hold off
225        %
226        % unconstrained estimate
227        x_free = xOut;
228        %
229        % write out constrained and unconstrained results
230        [fid, msg] = fopen('affine_ok_box.out', 'wt');
231        if size(msg, 2) > 0
232                disp(['affine_ok: ', msg]);
233        end
234        %                      123456789012345678901234'
235        heading =             '       t';
236        heading = [ heading , ' x2_true  x2_con x2_free' ];
237        heading = [ heading , '      z1\n'               ];
238        fprintf(fid, heading);
239        for k = 1 : N
240                fprintf(fid,'%8.3f%8.3f%8.3f%8.3f%8.3f\n', ...
241                        t(k), ...
242                        x_true(2,k), x_con(2,k), x_free(2,k), ...
243                        z(1,k) ...
244                );
245        end
246        fclose(fid);
247end
248return
249end
250% $$ $newlinech %$$
251% $end
Note: See TracBrowser for help on using the repository browser.