kfilter: /home/laradmin/lar/utils/kfilter/include/kfilter/ekfilter

00001 // This file is part of kfilter.
00002 // kfilter is a C++ variable-dimension extended kalman filter library.
00003 //
00004 // Copyright (C) 2004        Vincent Zalzal, Sylvain Marleau
00005 // Copyright (C) 2001, 2004  Richard Gourdeau
00006 // Copyright (C) 2004        GRPR and DGE's Automation sector
00007 //                           �cole Polytechnique de Montr�al
00008 //
00009 // Code adapted from algorithms presented in :
00010 //      Bierman, G. J. "Factorization Methods for Discrete Sequential
00011 //      Estimation", Academic Press, 1977.
00012 //
00013 // This library is free software; you can redistribute it and/or
00014 // modify it under the terms of the GNU Lesser General Public
00015 // License as published by the Free Software Foundation; either
00016 // version 2.1 of the License, or (at your option) any later version.
00017 //
00018 // This library is distributed in the hope that it will be useful,
00019 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00020 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00021 // Lesser General Public License for more details.
00022 //
00023 // You should have received a copy of the GNU Lesser General Public
00024 // License along with this library; if not, write to the Free Software
00025 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00026 
00027 #ifndef EKFILTER_IMPL_HPP
00028 #define EKFILTER_IMPL_HPP
00029 
00032 
00035 #define KALMAN_N_MODIFIED    1
00036 
00039 #define KALMAN_NU_MODIFIED  (1<<1)
00040 
00043 #define KALMAN_NW_MODIFIED  (1<<2)
00044 
00047 #define KALMAN_M_MODIFIED   (1<<3)
00048 
00051 #define KALMAN_NV_MODIFIED  (1<<4)
00052 
00055 #define KALMAN_P_MODIFIED   (1<<5)
00056 
00059 #define KALMAN_LOWMASK      ((1<<8) - 1)
00060 
00063 #define KALMAN_A_MODIFIED   (1<<8)
00064 
00067 #define KALMAN_W_MODIFIED   (1<<9)
00068 
00071 #define KALMAN_Q_MODIFIED   (1<<10)
00072 
00075 #define KALMAN_MIDMASK      ( ((1<<4) - 1) << 8 )
00076 
00079 #define KALMAN_H_MODIFIED   (1<<12)
00080 
00083 #define KALMAN_V_MODIFIED   (1<<13)
00084 
00087 #define KALMAN_R_MODIFIED   (1<<14)
00088 
00091 #define KALMAN_HIGHMASK     ( ((1<<4) - 1) << 12 )
00092 
00093 namespace Kalman {
00094 
00095   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00096   EKFilter<T, BEG, OQ, OVR, DBG>::EKFilter()
00097     : flags(0) {
00098                 setDim(0, 0, 0, 0, 0);
00099         }
00100 
00101   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00102   EKFilter<T, BEG, OQ, OVR, DBG>::EKFilter(K_UINT_32 n_, K_UINT_32 nu_, 
00103                                            K_UINT_32 nw_, K_UINT_32 m_, 
00104                                            K_UINT_32 nv_)
00105     : flags(0) {
00106     setDim(n_, nu_, nw_, m_, nv_);
00107   }
00108 
00109   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00110   EKFilter<T, BEG, OQ, OVR, DBG>::~EKFilter() {}
00111 
00112   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00113   void EKFilter<T, BEG, OQ, OVR, DBG>::setDim(K_UINT_32 n_, K_UINT_32 nu_, 
00114                                               K_UINT_32 nw_, K_UINT_32 m_, 
00115                                               K_UINT_32 nv_) {
00116     setSizeX(n_);
00117     setSizeU(nu_);
00118     setSizeW(nw_);
00119     setSizeZ(m_);
00120     setSizeV(nv_);
00121   }
00122 
00123   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00124   K_UINT_32 EKFilter<T, BEG, OQ, OVR, DBG>::getSizeX() const {
00125     return n;
00126   }
00127 
00128   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00129   K_UINT_32 EKFilter<T, BEG, OQ, OVR, DBG>::getSizeU() const {
00130     return nu;
00131   }
00132 
00133   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00134   K_UINT_32 EKFilter<T, BEG, OQ, OVR, DBG>::getSizeW() const {
00135     return nw;
00136   }
00137 
00138   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00139   K_UINT_32 EKFilter<T, BEG, OQ, OVR, DBG>::getSizeZ() const {
00140     return m;
00141   }
00142 
00143   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00144   K_UINT_32 EKFilter<T, BEG, OQ, OVR, DBG>::getSizeV() const {
00145     return nv;
00146   }
00147 
00148   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00149   void EKFilter<T, BEG, OQ, OVR, DBG>::setSizeX(K_UINT_32 n_) {
00150 
00151     // verify : n_ > 0
00152     if (n_ != n) {
00153       flags |= KALMAN_N_MODIFIED;
00154       n = n_;
00155     }
00156   }
00157 
00158   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00159   void EKFilter<T, BEG, OQ, OVR, DBG>::setSizeU(K_UINT_32 nu_) {
00160     if (nu_ != nu) {
00161       flags |= KALMAN_NU_MODIFIED;
00162       nu = nu_;
00163     }
00164   }
00165 
00166   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00167   void EKFilter<T, BEG, OQ, OVR, DBG>::setSizeW(K_UINT_32 nw_) {
00168     if (nw_ != nw) {
00169       flags |= KALMAN_NW_MODIFIED;
00170       nw = nw_;
00171     }
00172   }
00173 
00174   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00175   void EKFilter<T, BEG, OQ, OVR, DBG>::setSizeZ(K_UINT_32 m_) {
00176     if (m_ != m) {
00177       flags |= KALMAN_M_MODIFIED;
00178       m = m_;
00179     }
00180   }
00181 
00182   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00183   void EKFilter<T, BEG, OQ, OVR, DBG>::setSizeV(K_UINT_32 nv_) {
00184     if (nv_ != nv) {
00185       flags |= KALMAN_NV_MODIFIED;
00186       nv = nv_;
00187     }
00188   }
00189 
00190   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00191   void EKFilter<T, BEG, OQ, OVR, DBG>::init(Vector& x_, Matrix& P_) {
00192 
00193     // verify : (x_.size() == n && P_.nrow() == n && P_.ncol() == n)
00194 
00195     x.swap(x_);
00196     _P.swap(P_);
00197     flags |= KALMAN_P_MODIFIED;
00198   }
00199 
00200   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00201   void EKFilter<T, BEG, OQ, OVR, DBG>::step(Vector& u_, const Vector& z_) {
00202     timeUpdateStep(u_);
00203     measureUpdateStep(z_);
00204   }
00205 
00206   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00207   void EKFilter<T, BEG, OQ, OVR, DBG>::timeUpdateStep(Vector& u_) {
00208 
00209     // verif : u_.size() == nu
00210     K_UINT_32 i, j, k;
00211 
00212     sizeUpdate();
00213     u.swap(u_);
00214     
00215     makeCommonProcess();
00216     makeAImpl();
00217     makeWImpl();
00218     makeQImpl();
00219     makeProcess();
00220 
00221     if (!OQ) {
00222 
00223       if (flags & KALMAN_Q_MODIFIED) {
00224 
00225         Q_ = Q;
00226         factor(Q_);
00227         upperInvert(Q_);
00228 
00229       }
00230 
00231       Q.swap(Q_);
00232       
00233       // W_ = W*U   n.nw = n.nw * nw.nw
00234 
00235       if (flags & ( KALMAN_W_MODIFIED | KALMAN_Q_MODIFIED ) ) {
00236 
00237         for (i = BEG; i < n + BEG; ++i) {
00238     
00239           for (j = BEG; j < nw + BEG; ++j) {
00240       
00241             W_(i,j) = W(i,j);
00242             for (k = BEG; k < j; ++k)
00243               W_(i,j) += W(i,k)*Q(j,k);
00244       
00245           }
00246     
00247         }
00248   
00249       }
00250       
00251       W.swap(W_);
00252   
00253     }
00254 
00255     timeUpdate();
00256 
00257     if (!OQ) {
00258       Q.swap(Q_);
00259       W.swap(W_);
00260     }
00261 
00262     u.swap(u_);
00263     flags &= ~KALMAN_MIDMASK;
00264   }
00265 
00266   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00267   void EKFilter<T, BEG, OQ, OVR, DBG>::measureUpdateStep(const Vector& z_) {
00268 
00269     // verif : z_.size() == m
00270     K_UINT_32 i, j, k;
00271 
00272     sizeUpdate();
00273 
00274     if (m == 0) {
00275       return;
00276     }
00277     
00278     makeCommonMeasure();
00279     makeHImpl();
00280     makeVImpl();
00281     makeRImpl();    
00282     makeMeasure();
00283     
00284     // verif : nv != 0
00285 
00286     for (i = BEG; i < m + BEG; ++i)
00287       dz(i) = z_(i) - z(i);
00288 
00289     makeDZ();
00290 
00291     if (OVR) {
00292 
00293       // verif : m == nv
00294 
00295       if (flags & ( KALMAN_V_MODIFIED | KALMAN_R_MODIFIED ) ) {
00296 
00297         for (i = BEG; i < m + BEG; ++i)
00298           R_(i,i) = V(i,i)*V(i,i)*R(i,i);
00299 
00300       }
00301 
00302     } else {
00303 
00304 
00305       if (flags & ( KALMAN_V_MODIFIED | KALMAN_R_MODIFIED ) ) { // calculate R_
00306 
00307         _x.resize(nv);
00308       
00309         // R_ = V*R*V'
00310         for (i = BEG; i < m + BEG; ++i) {
00311 
00312           // _x = row i of V*R = (V*R)(i,:)
00313           for (j = BEG; j < nv + BEG; ++j) {
00314 
00315             _x(j) = T(0.0);
00316             for (k = BEG; k < nv + BEG; ++k)
00317               _x(j) += V(i,k)*R(k,j);
00318 
00319           }
00320 
00321           // R_(i,:) = (V*R*V')(i,:) = (V*R)(i,:) * V'
00322           for (j = BEG; j < m + BEG; ++j) {
00323 
00324             R_(i,j) = T(0.0);
00325             for (k = BEG; k < nv + BEG; ++k)
00326               R_(i,j) += _x(k)*V(j,k);
00327 
00328           }
00329 
00330         }
00331 
00332         // R_ = U*D*U'
00333         // diag(R_) = D, upper(R_) = U, lower(R_) = junk
00334         factor(R_);
00335 
00336         // lower(R_) = (inv(U))'
00337         upperInvert(R_);
00338 
00339       }
00340 
00341       if (flags & ( KALMAN_H_MODIFIED | KALMAN_V_MODIFIED | KALMAN_R_MODIFIED ) ) { // calculate H_
00342 
00343         // H_ = inv(U)*H    m.n = m.m * m.n
00344         for (i = BEG; i < m + BEG; ++i) {
00345 
00346           for (j = BEG; j < n + BEG; ++j) {
00347 
00348             H_(i,j) = H(i,j);
00349             for (k = i + 1; k < m + BEG; ++k)
00350               H_(i,j) += R_(k,i)*H(k,j);
00351 
00352           }
00353 
00354         }
00355 
00356       }
00357 
00358       H.swap(H_);
00359 
00360       // _x = inv(U)*dz    m.1 = m.m * m.1
00361       _x.resize(m);
00362 
00363       for (i = BEG; i < m + BEG; ++i) {
00364 
00365         _x(i) = dz(i);
00366         for (k = i + 1; k < m + BEG; ++k)
00367           _x(i) += R_(k,i)*dz(k); 
00368 
00369       }
00370 
00371       dz.swap(_x);
00372 
00373     }
00374     
00375     _x.resize(n); // dx : innovation
00376     _x = T(0.0);
00377     for (i = BEG; i < m + BEG; ++i) {
00378 
00379       for (j = BEG; j < n + BEG; ++j)
00380         a(j) = H(i,j);
00381 
00382       measureUpdate(dz(i), R_(i,i));
00383 
00384     }
00385     for (i = BEG; i < n + BEG; ++i)
00386       x(i) += _x(i);
00387 
00388     if (!OVR) {
00389       H.swap(H_);
00390     }
00391 
00392     flags &= ~KALMAN_HIGHMASK;
00393   }
00394 
00395   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00396   const KTYPENAME EKFilter<T, BEG, OQ, OVR, DBG>::Vector& EKFilter<T, BEG, OQ, OVR, DBG>::predict(Vector& u_) {
00397     
00398     // verif : u_.size() == nu
00399 
00400     sizeUpdate();
00401     u.swap(u_);   
00402     _x = x;
00403     
00404     makeCommonProcess();
00405     makeProcess();
00406     
00407     x.swap(_x);
00408     u.swap(u_);
00409     return _x;
00410   }
00411 
00412   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00413   const KTYPENAME EKFilter<T, BEG, OQ, OVR, DBG>::Vector& EKFilter<T, BEG, OQ, OVR, DBG>::simulate() {
00414     
00415     sizeUpdate();
00416     _x = z;
00417     
00418     makeCommonMeasure();
00419     makeMeasure();
00420     
00421     z.swap(_x);
00422     return _x;
00423   }
00424 
00425   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00426   const KTYPENAME EKFilter<T, BEG, OQ, OVR, DBG>::Vector& EKFilter<T, BEG, OQ, OVR, DBG>::getX() const {
00427     return x;
00428   }
00429 
00430   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00431   const KTYPENAME EKFilter<T, BEG, OQ, OVR, DBG>::Matrix& EKFilter<T, BEG, OQ, OVR, DBG>::calculateP() const {
00432 
00433     if (!(flags & KALMAN_P_MODIFIED)) {
00434 
00435       _P.resize(n, n);         // keep this resize
00436     
00437       for (K_UINT_32 i = BEG; i < n + BEG; ++i) {
00438 
00439         _P(i,i) = U(i,i);
00440 
00441         for (K_UINT_32 j = i + 1; j < n + BEG; ++j) {
00442 
00443           _P(i,j)  = U(i,j)*U(j,j);
00444           _P(i,i) += U(i,j)*_P(i,j);
00445 
00446           for (K_UINT_32 k = j + 1; k < n + BEG; ++k) {
00447             _P(i,j) += U(i,k)*U(j,k)*U(k,k);
00448           }
00449 
00450           _P(j,i) = _P(i,j);
00451 
00452         }
00453 
00454       }
00455 
00456     }
00457 
00458     return _P;
00459   }
00460 
00461   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00462   void EKFilter<T, BEG, OQ, OVR, DBG>::NoModification() {
00463     modified_ = false;
00464   }
00465 
00466   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00467   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseA() {
00468     NoModification();
00469   }
00470   
00471   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00472   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseW() {
00473     NoModification();
00474   }
00475 
00476   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00477   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseQ() {
00478     NoModification();
00479   }
00480   
00481   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00482   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseH() {
00483     NoModification();
00484   }
00485 
00486   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00487   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseV() {
00488     NoModification();
00489   }
00490 
00491   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00492   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseR() {
00493     NoModification();
00494   }
00495 
00496   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00497   void EKFilter<T, BEG, OQ, OVR, DBG>::makeCommonProcess() {}
00498 
00499   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00500   void EKFilter<T, BEG, OQ, OVR, DBG>::makeCommonMeasure() {}
00501 
00502   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00503   void EKFilter<T, BEG, OQ, OVR, DBG>::makeA() {
00504     NoModification();
00505   }
00506   
00507   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00508   void EKFilter<T, BEG, OQ, OVR, DBG>::makeW() {
00509     NoModification();
00510   }
00511 
00512   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00513   void EKFilter<T, BEG, OQ, OVR, DBG>::makeQ() {
00514     NoModification();
00515   }
00516   
00517   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00518   void EKFilter<T, BEG, OQ, OVR, DBG>::makeH() {
00519     NoModification();
00520   }
00521 
00522   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00523   void EKFilter<T, BEG, OQ, OVR, DBG>::makeV() {
00524     NoModification();
00525   }
00526 
00527   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00528   void EKFilter<T, BEG, OQ, OVR, DBG>::makeR() {
00529     NoModification();
00530   }
00531 
00532   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00533   void EKFilter<T, BEG, OQ, OVR, DBG>::makeDZ() {}
00534 
00535   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00536   void EKFilter<T, BEG, OQ, OVR, DBG>::sizeUpdate() {
00537       
00538     if (!flags) {
00539       return;
00540     }
00541 
00542     if (flags & KALMAN_N_MODIFIED) {
00543       A.resize(n, n);
00544       makeBaseAImpl();
00545     }
00546     
00547     if (flags & (KALMAN_N_MODIFIED | KALMAN_NW_MODIFIED) ) {
00548       nn = n + nw;
00549       a.resize(nn);
00550       v.resize(nn);
00551       d.resize(nn);
00552       if (!OQ)
00553         W_.resize(n, nw);
00554       W.resize(n, nw);
00555       makeBaseWImpl();
00556     }
00557 
00558     // KALMAN_N_MODIFIED imply KALMAN_P_MODIFIED
00559     // => KALMAN_N_MODIFIED must not be set OR KALMAN_P_MODIFIED must be set
00560     // => NOT  KALMAN_N_MODIFIED  OR  KALMAN_P_MODIFIED  must be set
00561     // verif : (flags ^ KALMAN_N_MODIFIED) & 
00562     //              (KALMAN_N_MODIFIED | KALMAN_P_MODIFIED)
00563 
00564     if (flags & KALMAN_P_MODIFIED) { 
00565       // this covers the case of KALMAN_N_MODIFIED = true also
00566 
00567       // We have a new matrix P : let's factorize it and store it in U
00568       // First, resize U and copy P in its left part
00569       U.resize(n, nn);
00570       for (K_UINT_32 i = BEG; i < n + BEG; ++i)
00571         for (K_UINT_32 j = BEG; j < n + BEG; ++j)
00572           U(i,j) = _P(i,j);
00573       
00574       // Factorize
00575       factor(U);
00576 
00577     } else if (flags & KALMAN_NW_MODIFIED) {
00578       // KALMAN_N_MODIFIED is necessarily false, else KALMAN_P_MODIFIED
00579       // would have been true
00580 
00581       // Let's just copy U in temporary matrix _P of the right size,
00582       // then swap the matrices
00583       _P.resize(n, nn);
00584       for (K_UINT_32 i = BEG; i < n + BEG; ++i)
00585         for (K_UINT_32 j = i; j < n + BEG; ++j)
00586           _P(i,j) = U(i,j);
00587       U.swap(_P);
00588     }
00589 
00590     if (flags & KALMAN_NW_MODIFIED) {
00591       if (!OQ)
00592         Q_.resize(nw, nw);
00593       Q.resize(nw, nw);
00594       makeBaseQImpl();
00595     }
00596 
00597     if (m != 0) {
00598 
00599       if (flags & (KALMAN_N_MODIFIED | KALMAN_M_MODIFIED) ) {
00600         if (!OVR)
00601           H_.resize(m, n);
00602         H.resize(m, n);
00603         makeBaseHImpl();
00604       }
00605 
00606       if (flags & (KALMAN_M_MODIFIED | KALMAN_NV_MODIFIED) ) {
00607         V.resize(m, nv);
00608         makeBaseVImpl();
00609       }
00610 
00611       if (flags & KALMAN_NV_MODIFIED) {
00612         R.resize(nv, nv);
00613         makeBaseRImpl();
00614       }
00615 
00616       if (flags & KALMAN_M_MODIFIED) {
00617         R_.resize(m, m);
00618         z.resize(m);
00619         dz.resize(m);
00620       }
00621 
00622     }
00623     
00624     flags &= ~KALMAN_LOWMASK;
00625   }
00626 
00627   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00628   void EKFilter<T, BEG, OQ, OVR, DBG>::factor(Matrix& P_) {
00629 
00630     // ne pas v�rifier que P_.ncol() == P_.nrow(), comme �a, m�me si
00631     // nrow() < ncol(), on peut factoriser la sous-matrice carr�e de P
00632     // Utile pour factoriser U
00633 
00634     T alpha, beta;
00635     K_UINT_32 i, j, k, N = P_.nrow();
00636     for(j = N - 1 + BEG; j > BEG; --j) {
00637       alpha = T(1.0)/P_(j,j);
00638       for(k = BEG; k < j; ++k) {
00639         beta = P_(k,j);
00640         P_(k,j) = alpha*beta;
00641         for(i = BEG; i <= k; ++i)
00642           P_(i,k) -= beta*P_(i,j);
00643       }
00644     }
00645   }
00646 
00647   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00648   void EKFilter<T, BEG, OQ, OVR, DBG>::upperInvert(Matrix& P_) {
00649 
00650     T val;
00651     K_UINT_32 i, j, k, N = P_.nrow();
00652     for (i = N - 2 + BEG; i != (K_UINT_32)(BEG-1); --i) { // intended overflow if BEG==0
00653       for (k = i + 1; k < N + BEG; ++k) {
00654 
00655         val = P_(i,k);
00656         for (j = i + 1; j <= k - 1; ++j)
00657           val += P_(i,j)*P_(k,j);
00658         P_(k,i) = -val;
00659 
00660       }
00661     }
00662 
00663   }
00664 
00665   // U    u     U-D covariance matrix (n,nn)
00666   // A    phi   transition matrix (F) (n,n)
00667   // W    g     process noise matrix (G) (n,nw)
00668   // Q    q     process noise variance vector (nw) Q = diag(q)
00669   // a, v, d temporary vectors
00670   // U is updated
00671   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00672   void EKFilter<T, BEG, OQ, OVR, DBG>::timeUpdate() {
00673 
00674     K_UINT_32 i, j, k;
00675     T sigma, dinv;
00676   
00677     // U = phi * U
00678     // d = diag(U)
00679     // 
00680     // This algo could be faster
00681     // if phi is known to be diagonal
00682     // It could be almost zapped if phi=I
00683     for(j = n - 1 + BEG; j > BEG; --j) {
00684       for(i = BEG; i <= j; ++i)
00685         d(i) = U(i,j);
00686       for(i = BEG; i < n + BEG; ++i) {
00687         U(i,j) = A(i,j);
00688         for(k = BEG; k < j; ++k)
00689           U(i,j) += A(i,k)*d(k);
00690       }
00691     }
00692 
00693     d(BEG) = U(BEG,BEG);
00694     for(j = BEG; j < n + BEG; ++j)
00695       U(j,BEG) = A(j,BEG);
00696 
00697     // d(n+1:nn) = q 
00698     // U(:,n+1:nn) = G 
00699     for(i = BEG; i < nw + BEG; ++i) {
00700       d(i+n) = Q(i,i);
00701       for(j = BEG; j < n + BEG; ++j)
00702         U(j,i+n) = W(j,i);
00703     }
00704 
00705     // Gram-Schmidt
00706     // Too hard to simplify
00707     for(j = n - 1 + BEG; j != (K_UINT_32)(BEG-1); --j) { // intended overflow if BEG==0
00708       sigma = T(0.0);
00709       for(k = BEG; k < nn + BEG; ++k) {
00710         v(k) = U(j,k);
00711         a(k) = d(k)*v(k);
00712         sigma += v(k)*a(k);
00713       }
00714       U(j,j) = sigma;
00715       if(j == BEG || sigma == T(0.0)) continue;
00716       dinv = T(1.0)/sigma;
00717       for(k = BEG; k < j; ++k) {
00718         sigma = T(0.0);
00719         for(i = BEG; i < nn + BEG; ++i) 
00720           sigma += U(k,i)*a(i);
00721         sigma *= dinv;
00722         for(i = BEG; i < nn + BEG; ++i) 
00723           U(k,i) -= sigma*v(i);
00724         U(j,k) = sigma;
00725       }
00726     }
00727 
00728     // U = transpose(U)
00729     for(j = BEG + 1; j < n + BEG; ++j)
00730       for(i = BEG; i < j; ++i)
00731         U(i,j) = U(j,i);
00732   }
00733 
00734   // x     a priori estimate vector (n)
00735   // U     a priori U-D covariance matrix (n,nn)
00736   // dz    measurement diff (z - ax) (scalar)
00737   // a     measurement coefficients vector (n) (a row of A, which is H)
00738   //          a is destroyed
00739   // r     measurement variance
00740   // d is a temporary vector
00741   // x and U are updated
00742   // a is destroyed
00743   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00744   void EKFilter<T, BEG, OQ, OVR, DBG>::measureUpdate(T dz, T r) {
00745 
00746     K_UINT_32 i, j, k;
00747     T alpha, gamma, beta, lambda;
00748 
00749     // dz = dz - Hdx
00750     for (j = BEG; j < n + BEG; ++j)
00751       dz -= a(j)*_x(j);
00752     
00753     // d = D * transpose(U) * a
00754     // a =     transpose(U) * a
00755     //
00756     // This algo could be faster
00757     // if A is known to be diagonal or I
00758     for(j = n - 1 + BEG; j > BEG; --j) {
00759       for(k = BEG; k < j; ++k)
00760         a(j) += U(k,j)*a(k);
00761       d(j) = U(j,j)*a(j);
00762     }
00763     d(BEG) = U(BEG,BEG)*a(BEG);
00764 
00765     // UDU
00766     // Too hard to simplify
00767     alpha = r+d(BEG)*a(BEG);
00768     gamma = T(1.0)/alpha;
00769     U(BEG,BEG) = r*gamma*U(BEG,BEG);
00770     for(j = BEG + 1; j < n + BEG; ++j) {
00771       beta = alpha;
00772       alpha += d(j)*a(j);
00773       lambda = -a(j)*gamma;
00774       gamma = T(1.0)/alpha;
00775       U(j,j) *= beta*gamma;
00776       for(i = BEG; i < j; ++i) {
00777         beta = U(i,j);
00778         U(i,j) = beta+d(i)*lambda;
00779         d(i) += d(j)*beta;
00780       }
00781     }
00782   
00783     // dx = dx + K(dz - Hdx)
00784     dz *= gamma;
00785     for(j = BEG; j < n + BEG; ++j)
00786       _x(j) += d(j)*dz;
00787   }
00788 
00789   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00790   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseAImpl() {
00791     modified_ = true;
00792     makeBaseA();
00793     if (modified_)
00794       flags |= KALMAN_A_MODIFIED;
00795   }
00796   
00797   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00798   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseWImpl() {
00799     modified_ = true;
00800     makeBaseW();
00801     if (modified_)
00802       flags |= KALMAN_W_MODIFIED;    
00803   }
00804   
00805   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00806   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseQImpl() {
00807     modified_ = true;
00808     makeBaseQ();
00809     if (modified_)
00810       flags |= KALMAN_Q_MODIFIED;    
00811   }
00812   
00813   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00814   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseHImpl() {
00815     modified_ = true;
00816     makeBaseH();
00817     if (modified_)
00818       flags |= KALMAN_H_MODIFIED;    
00819   }
00820   
00821   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00822   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseVImpl() {
00823     modified_ = true;
00824     makeBaseV();
00825     if (modified_)
00826       flags |= KALMAN_V_MODIFIED;    
00827   }
00828   
00829   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00830   void EKFilter<T, BEG, OQ, OVR, DBG>::makeBaseRImpl() {
00831     modified_ = true;
00832     makeBaseR();
00833     if (modified_)
00834       flags |= KALMAN_R_MODIFIED;    
00835   }
00836   
00837   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00838   void EKFilter<T, BEG, OQ, OVR, DBG>::makeAImpl() {
00839     modified_ = true;
00840     makeA();
00841     if (modified_)
00842       flags |= KALMAN_A_MODIFIED;    
00843   }
00844   
00845   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00846   void EKFilter<T, BEG, OQ, OVR, DBG>::makeWImpl() {
00847     modified_ = true;
00848     makeW();
00849     if (modified_)
00850       flags |= KALMAN_W_MODIFIED;    
00851   }
00852   
00853   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00854   void EKFilter<T, BEG, OQ, OVR, DBG>::makeQImpl() {
00855     modified_ = true;
00856     makeQ();
00857     if (modified_)
00858       flags |= KALMAN_Q_MODIFIED;    
00859   }
00860   
00861   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00862   void EKFilter<T, BEG, OQ, OVR, DBG>::makeHImpl() {
00863     modified_ = true;
00864     makeH();
00865     if (modified_)
00866       flags |= KALMAN_H_MODIFIED;    
00867   }
00868   
00869   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00870   void EKFilter<T, BEG, OQ, OVR, DBG>::makeVImpl() {
00871     modified_ = true;
00872     makeV();
00873     if (modified_)
00874       flags |= KALMAN_V_MODIFIED;    
00875   }
00876 
00877   template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
00878   void EKFilter<T, BEG, OQ, OVR, DBG>::makeRImpl() {
00879     modified_ = true;
00880     makeR();
00881     if (modified_)
00882       flags |= KALMAN_R_MODIFIED;    
00883   }
00884 
00885 }
00886 
00887 #endif
/home/laradmin/lar/utils/kfilter/include/kfilter/ekfilter_impl.hpp
