# source:projects/ckbs/trunk/test/nonlinear_ok_simple.m@95

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

File size: 3.8 KB
Line
1% -------------------------------------------------------------------
2% ckbs: Constrained Kalman-Bucy Smoother Program: Copyright (C) 2006
4%          Gianluigi Pillonetto: giapi at dei dot unipd dot it
6% -------------------------------------------------------------------
7% \$begin nonlinear_ok_simple.m\$\$ \$newlinech %\$\$
8% \$spell
9%       itr
10%       uk
11%       dk
12%       ckbs
13%       qinv
14%       qinvk
15%       rinv
16%       rinvk
17%       xk
18%       ek
19%       df
20%       dg
21%       dh
22%       Fk
23%       Gk
24%       Hk
25%       feval
26%       sumsq
27% \$\$
28%
29% \$section Simple ckbs_nonlinear Example and Test\$\$
30%
31% \$index ckbs, example and test\$\$
32% \$index example, ckbs_nonlinear\$\$
33% \$index test, ckbs_nonlinear\$\$
34%
35% \$children%
36%       test/nonlinear_ok_simple_f.m%
37%       test/nonlinear_ok_simple_g.m%
38%       test/nonlinear_ok_simple_h.m
39% %\$\$
41% \$table
42% \$rref nonlinear_ok_simple_f.m\$\$
43% \$rref nonlinear_ok_simple_g.m\$\$
44% \$rref nonlinear_ok_simple_h.m\$\$
45% \$tend
46%
48% \$newlinech \$\$ \$codep
49function [ok] = nonlinear_ok_simple()
50% --------------------------------------------------------------------
51max_itr = 20;      % maximum number of iterations
52epsilon = 1e-5;    % convergence criteria
53N       = 3;       % number of time points
54ell     = 2;       % number of constraints
55m       = 1;       % number of measurements per time point
56n       = 3;       % number of state vector components per time point
57sigma   = 1;       % variance of measurement noise
58level   = 0;       % level of tracing in ckbs_nonlinear
59%
60% simulated true trajectory
61x_true  = ones(n, 1) * (1 : N);
62%
63% simulate measurement noise
64e_true  = zeros(m, N);
65%
66% initial x vector
67x_in = zeros(n, N);
68%
69% --------------------------------------------------------------------
70if m > n
71        error('nonlinear_ok_simple: m > n');
72end
73if ell > n
74        error('nonlinear_ok_simple: ell > n');
75end
76%
77global nonlinear_ok_simple_m
78global nonlinear_ok_simple_ell
79global nonlinear_ok_simple_N
80%
81nonlinear_ok_simple_m   = m;
82nonlinear_ok_simple_ell = ell;
83nonlinear_ok_simple_N   = N;
84%
85% measurement variances
86rinv    = zeros(m, m, N);
87qinv    = zeros(n, n, N);
88qinvk   = eye(n) / (sigma * sigma);
89rinvk   = eye(m) / (sigma * sigma);
90z       = zeros(m, N);
91for k = 1 : N
92        xk           = x_true(:, k);
93        ek           = e_true(:, k);
94        hk           = nonlinear_ok_simple_h(k, xk);
95        z(:, k)      = hk + ek;
96        qinv(:,:, k) = qinvk;
97        rinv(:,:, k) = rinvk;
98end
99%
100% pass the maximum state value to f
101global nonlinear_ok_simple_f_max
102nonlinear_ok_simple_f_max = (N - .5) * ones(m, 1);
103%
104% ----------------------------------------------------------------------
105f_fun = 'nonlinear_ok_simple_f';
106g_fun = 'nonlinear_ok_simple_g';
107h_fun = 'nonlinear_ok_simple_h';
108[x_out, u_out, info] = ckbs_nonlinear( ...
109        f_fun,    ...
110        g_fun,    ...
111        h_fun,    ...
112        max_itr,  ...
113        epsilon,  ...
114        x_in,     ...
115        z,        ...
116        qinv,     ...
117        rinv,     ...
118        level     ...
119);
120% ----------------------------------------------------------------------
121ok     = size(info, 1) <= max_itr;
122f_out  = zeros(ell, N);
123g_out  = zeros(n,   N);
124h_out  = zeros(m,   N);
125df_out = zeros(ell, n, N);
126dg_out = zeros(n, n,   N);
127dh_out = zeros(m, n,   N);
128xk1    = zeros(n, 1);
129for k = 1 : N
130        xk       = x_out(:, k);
131        uk       = u_out(:, k);
132        [fk, Fk] = feval(f_fun, k, xk);
133        [gk, Gk] = feval(g_fun, k, xk1);
134        [hk, Hk] = feval(h_fun, k, xk);
135        %
136        ok       = ok & all( fk <= epsilon);
137        ok       = ok & all( abs(fk) .* uk <= epsilon );
138        %
139        f_out(:, k) = fk - Fk * xk;
140        g_out(:, k) = gk - Gk * xk1;
141        h_out(:, k) = hk - Hk * xk;
142        df_out(:,:, k) = Fk;
143        dg_out(:,:, k) = Gk;
144        dh_out(:,:, k) = Hk;
145        xk1 = xk;
146end
147ok   = ok & all( all( u_out >= 0 ) );