# source:projects/ckbs/trunk/src/ckbs_affine_singular.m@635

Last change on this file since 635 was 635, checked in by aleksand, 22 months ago

Singular smoother works

File size: 7.8 KB
Line
1% -------------------------------------------------------------------
2% ckbs: Constrained Kalman-Bucy Smoother Program: Copyright (C) 2006-2011
4%          Gianluigi Pillonetto: giapi at dei dot unipd dot it
5%          Aleksandr Y. Aravkin: sasha.aravkin at gmail dot com
7% -------------------------------------------------------------------
8% $begin ckbs_affine_singular$$newlinech %$$ 9%$spell
10%       itr
11%       obj
12%       ckbs
13%       dg
14%       dh
15%       qinv
16%       rinv
17%       optimality
18%       pairwise
19% $$20% 21% index ckbs_affine_singular$$
22%
23% $index affine, singular smoother$$24% index singular, affine smoother$$ 25%$index smoother, affine singular$$26% 27% section Singular Affine Kalman Bucy Smoother$$
28%
29% $head Syntax$$30% codei/[/xOut/, /info/] = ckbs_affine_singular(... 31% /z/, /g/, /h/, ... 32% /dg/, /dh/, /q/, /r/)/$$ 33% 34%$head Purpose$$35% This routine finds the smoothed estimate for affine process and measurement 36% models when the variance matrices latex Q$$ and $latex R$$may be singular. 37% 38% 39% head Notation$$ 40% The singular Kalman-Bucy smoother state is given by 41%$latex $42% \begin{array}{rcl} 43% G^{-1} (w - Q \Phi^{-1} (GH^T (H H^T)^{-1} z - g)) 44% \end{array} 45%$ $$46% where 47%  latex $48% \Phi = Q + GH^T (H H^T)^{-1} R (H H^T) ^{-1} H G^T , 49%$$$
50% and the matrices $latex R_k$$and latex Q_k$$ are 51% symmetric positive semidefinite. 52% Note that$latex g_1$$is the initial state estimate 53% and latex Q_1$$ is the corresponding covariance.
54%
55%
56% $head z$$57% The argument icode z$$ is a two dimensional array, 58% for$latex k = 1 , \ldots , N$$59% latex $60% z_k = z(:, k) 61%$$$
62% and $icode z$$has size latex m \times N$$. 63% 64%$head g$$65% The argument icode g$$ is a two dimensional array,
66% for $latex k = 1 , \ldots , N$$67% latex $68% g_k = g(:, k) 69%$$$ 70% and$icode g$$has size latex n \times N$$.
71% The value $latex g_1$$serves as the initial state estimate. 72% 73% head h$$ 74% The argument$icode h$$is a two dimensional array, 75% for latex k = 1 , \ldots , N$$
76% $latex $77% h_k = h(:, k) 78%$$$79% and icode h$$ has size$latex m \times N$$. 80% 81% 82% head dg$$
83% The argument $icode dg$$is a three dimensional array, 84% for latex k = 1 , \ldots , N$$ 85%$latex $86% G_k = dg(:,:,k) 87%$$$88% and icode dg$$ has size $latex n \times n \times N$$. 89% The initial state estimate latex g_1$$ does not depend on the value of 90%$latex x_0$$, hence latex G_1$$ should be zero.
91%
92% $head dh$$93% The argument icode dh$$ is a three dimensional array, 94% for$latex k = 1 , \ldots , N$$95% latex $96% H_k = dh(:,:,k) 97%$$$
98% and $icode dh$$has size latex m \times n \times N$$. 99% 100%$head q$$101% The argument icode q$$ is a three dimensional array,
102% for $latex k = 1 , \ldots , N$$103% latex $104% Q_k = q(:,:, k) 105%$$$ 106% and$icode q$$has size latex n \times n \times N$$.
107% The value of $latex Q_k$$is the variance of the initial state 108% estimate latex g_1$$. 109% 110%$head r$$111% The argument icode r$$ is a three dimensional array,
112% for $latex k = 1 , \ldots , N$$113% latex $114% R_k = r(:,:, k) 115%$$$ 116% and$icode r$$has size latex m \times m \times N$$.
117% It is ok to signify a missing data value by setting the corresponding
118% row and column of $icode r$$to infinity. This also enables use 119% to interpolate the state vector latex x_k$$ to points where 120% there are no measurements. 121% 122%$head xOut$$123% The result icode xOut$$ contains the optimal sequence
124% $latex ( x_1 , \ldots , x_N )$$. 125% For latex k = 1 , \ldots , N$$ 126%$latex $127% x_k = xOut(:, k) 128%$$$129% and icode xOut$$ is a two dimensional array with size $latex n \times N$$. 130% 131% 132% head info$$ 133% Contains infinity norm of each of the three KKT equations for the 134% constrained reformulation. 135% %$$136% pre 137% 138%$$ 139% 140%$children#
141%       example/affine_singular_ok.m
142% #$$143% 144% head Example$$
145% The file $cref/affine_singular_ok.m/$$contains an example and test of 146% code ckbs_affine_singular$$. 147% It returns true if$code ckbs_affine_singular passes the test
148% and false otherwise.
149%
150% \$end
151% ----------------------------------------------------------------------------
152
153
154function [xOut, info] = ckbs_affine_singular(z, g, h, dg, dh, q, r)
155
156    info = [];
157    if nargin ~= 7
158        error('ckbs_affine: improper number of input arguments');
159    end
160    if nargout ~= 2
161        error('ckbs_affine: improper number of return values');
162    end
163    % size of problem
164    n     = size(g, 1);
165    N     = size(z, 2);
166    m     = size(z,   1);
167    %ell   = size(b,   1);
168    %
169    % check sizes
170    if  N ~= size(h,2) || N ~= size(dg,3) || N ~= size(dh,3) || N ~= size(q,3) || N ~= size(r,3)
171        N
172        size(z,2)
173        size(h,2)
174        size(dg,3)
175        size(dh,3)
176        size(q,3)
177        size(r,3)
178        error('ckbs_affine: argument sizes do not agree');
179    end
180    if n ~= size(dg,2) || n ~= size(dh,2) || ...
181            n ~= size(q,1) ||n ~= size(q,2)
182        n
183        size(g,1)
184        size(dg,2)
185        size(dh,2)
186        size(q,1)
187        size(q,2)
188        error('ckbs_affine: argument sizes do not agree');
189    end
190    if m ~= size(h,1) | m ~= size(dh,1) | m ~= size(r,1) | m ~= size(r,2)
191        m
192        size(h,1)
193        size(dh,1)
194        size(r,1)
195        size(r,2)
196        error('ckbs_affine: argument sizes do not agree');
197    end
198    %
199    % Other usefull sizes
200    %meas        = ell * N;
201
202
203
204
205    packedId = zeros(n, n, N);
206    for k = 1:N
207       packedId(:,:,k) = eye(n);
208    end
209
210    w = g(:);
211    v = z(:);
212
213    GinvW =  ckbs_bidiag_solve(packedId, -dg, w);
214    HGinvWmV =  ckbs_blkdiag_mul(dh, GinvW) - v;
215
216    lambdaTwo = pcg(@(x)specialAction(x, dh, dg, q, r), HGinvWmV, [], n*N);
217
218    lambdaOne = ckbs_blkdiag_mul_t(dh, lambdaTwo);
219    lambdaOne = -ckbs_bidiag_solve_t(packedId, -dg, lambdaOne);
220
221    QlamOne = ckbs_blkdiag_mul(q, lambdaOne);
222    xOut = ckbs_bidiag_solve(packedId, -dg, w + QlamOne);
223    xOut = reshape(xOut, n, N);
224
225%     zv = z(:);
226%
227%     iHH = zeros(m, m, N);
228%     HiRiH = zeros (n, n, N);
229%
230%     for k = 1:N
231%        packedId(:,:,k) = eye(n);
232%        iHH(:,:,k) = inv(dh(:,:,k) * dh(:,:,k)'); % the HH^T matrix
233%        HiRiH(:,:,k) = dh(:,:,k)' * iHH(:,:,k) * r(:,:,k) * iHH(:,:,k) * dh(:,:,k);
234%     end
235%
236%     iHHz = ckbs_blkdiag_mul(iHH, zv);
237%     HiHHz = ckbs_blkdiag_mul_t(dh, iHHz);
238%     GHiHHz = ckbs_blkbidiag_mul(packedId, -dg, HiHHz);
239%
240%     w = g(:);
241%
242%     rhs = GHiHHz - w;
243%
244%     [PhiDia, PhiOffDia] = ckbs_blkbidiag_symm_mul(packedId, -dg, HiRiH);
245%     PhiDia = PhiDia + q;
246%
247%     lambdaOne = -ckbs_tridiag_solve_b(PhiDia, PhiOffDia, rhs);
248%
249%     QlambdaOne = ckbs_blkdiag_mul(q, lambdaOne);
250%
251%     xOut = ckbs_bidiag_solve(packedId, -dg, w - QlambdaOne);
252%
253%     xOut = reshape(xOut, n, N);
254%
255    info = [0 0]; % temporary holder
256
257    % KKT check
258    GtlambdaOne = ckbs_blkbidiag_mul_t(packedId, -dg, lambdaOne);
259    HGtlambdaOne = ckbs_blkdiag_mul(dh, GtlambdaOne);
260    %lambdaTwo = -ckbs_blkdiag_mul(iHH, HGtlambdaOne);
261
262    K0 = specialAction(lambdaTwo, dh, dg, q, r) - HGinvWmV;
263    K1 = ckbs_blkbidiag_mul(packedId, -dg, xOut(:)) - ckbs_blkdiag_mul(q, lambdaOne) - w;
264    K2 = ckbs_blkdiag_mul(dh, xOut(:)) - ckbs_blkdiag_mul(r, lambdaTwo) - v;
265    K3 = GtlambdaOne + ckbs_blkdiag_mul_t(dh, lambdaTwo);
266
267    info = [norm(K0, inf); norm(K1, inf); norm(K2, inf); norm(K3, inf)];
268
269return
270end
271
272function [v] = specialAction(x, dh, dg, q, r)
273
274n = size(dg, 1);
275N = size(dg, 3);
276packedId = zeros(n, n, N);
277for k = 1:N
278    packedId(:,:,k) = eye(n);
279end
280
281v = ckbs_blkdiag_mul(r, x);
282temp = ckbs_blkdiag_mul_t(dh, x);
283temp = ckbs_bidiag_solve_t(packedId, -dg, temp);
284temp = ckbs_blkdiag_mul(q, temp);
285temp = ckbs_bidiag_solve(packedId, -dg, temp);
286temp = ckbs_blkdiag_mul(dh, temp);
287
288v = v + temp;
289end
Note: See TracBrowser for help on using the repository browser.