fix srkf
[scilab.git] / scilab / modules / signal_processing / macros / srkf.sci
1 // Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
2 // Copyright (C) INRIA - 1988 - C. Bunks
3 //
4 // This file must be used under the terms of the CeCILL.
5 // This source file is licensed as described in the file COPYING, which
6 // you should have received as part of this distribution.  The terms
7 // are also available at
8 // http://www.cecill.info/licences/Licence_CeCILL_V2.1-en.txt
9
10 function [x1,p1]=srkf(y,x0,p0,f,h,q,r)
11     //square root kalman filter algorithm
12     //Input to the macro is:
13     // f,h    :current system matrices
14     // q,r    :covariance matrices of dynamics
15     //        :and observation noise
16     // x0,p0  :state estimate and error variance
17     //        :at t=0 based on data up to t=-1
18     // y      :current observation
19     //
20     //Output from the macro is:
21     // x1,p1  :updated estimate and error covariance
22     //        :at t=1 based on data up to t=0
23     //!
24
25     n=max(size(x0));
26     p=max(size(y));
27
28     j=[chol(r)',0*r];
29     g=[0*q,chol(q)'];
30
31     mat=[h*p0,j;f*p0,g];
32     [q,tmat]=qr(mat');
33     tmat=tmat';
34     p1=tmat(p+1:p+n,p+1:p+n);
35     k=tmat(p+1:p+n,1:p);
36     re=tmat(1:p,1:p);
37
38     epsilon=y-h*x0;
39     x1=f*x0+k*(re**(-1))*epsilon;
40 endfunction