[Toon-members] tag/tag unscented.h

[Gerhard Reitmayr]

CVSROOT:        /cvsroot/toon
Module name:    tag
Changes by:     Gerhard Reitmayr <gerhard>      07/12/11 15:41:08

Modified files:
        tag            : unscented.h 

Log message:
        made Sqrt versions accessible, removed some dependencies

CVSWeb URLs:
http://cvs.savannah.gnu.org/viewcvs/tag/tag/unscented.h?cvsroot=toon&r1=1.1&r2=1.2

Patches:
Index: unscented.h
===================================================================
RCS file: /cvsroot/toon/tag/tag/unscented.h,v
retrieving revision 1.1
retrieving revision 1.2
diff -u -b -r1.1 -r1.2
--- unscented.h 3 Dec 2007 17:39:31 -0000       1.1
+++ unscented.h 11 Dec 2007 15:41:07 -0000      1.2
@@ -9,24 +9,27 @@
 /// @defgroup unscented Unscented Transform
 /// Implementations of the unscented transform for different sigma point 
selections. The @ref sphericalUnscentedTransform
 /// will use only N + 2 sigma points (N dimension of input), while the @ref 
unscentedTransform uses 2N + 1 sigma points with
-/// a simpler algorithm for sigma points. For expensive functions the 
difference in evaluated points may matter.
+/// a simpler algorithm for sigma points. The @ref sphericalUnscentedTransform 
incurrs slight overhead in computing the sigma points,
+/// but for expensive functions the difference in number evaluated points can 
win.
 
 namespace Internal {
 
-template <class F, int N, class V, class M> void 
outer_product_upper_half(const TooN::FixedVector<N,V>& v, const double w, 
TooN::FixedMatrix<N,N,M>& m)
+template <class F, int N, class V, class M> inline void 
outer_product_upper_half(const TooN::FixedVector<N,V>& v, const double w, 
TooN::FixedMatrix<N,N,M>& m)
 {
     for (int i=0; i<N; ++i)
         for (int j=i; j<N; ++j)
             F::eval(m[i][j], w * v[i] * v[j]);
 }
 
-template <class F, int N, class V1, class V2, class M> void 
outer_product_upper_half(const TooN::FixedVector<N,V1>& v1, const 
TooN::FixedVector<N,V2>& v2, const double w, TooN::FixedMatrix<N,N,M>& m)
+template <class F, int N, class V1, class V2, class M> inline void 
outer_product_upper_half(const TooN::FixedVector<N,V1>& v1, const 
TooN::FixedVector<N,V2>& v2, const double w, TooN::FixedMatrix<N,N,M>& m)
 {
     for (int i=0; i<N; ++i)
         for (int j=i; j<N; ++j)
             F::eval(m[i][j], w * (v1[i] * v1[j] + v2[i] * v2[j]));
 }
 
+}
+
 template <int N, int M, class F, class V1, class V2, class M1, class M2>
 void unscentedTransformSqrt(const TooN::FixedVector<N, V1>& x, const 
TooN::FixedMatrix<N,N,M1>& L, const F& f, TooN::FixedVector<M,V2>& newx, 
TooN::FixedMatrix<M,M,M2>& newP)
 {
@@ -35,15 +38,15 @@
     static const double spread = sqrt(N/(1-w0));
     const TooN::Vector<M> y0 = f(x);
     newx = w0 * y0;
-    outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
+    Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
     for (int i=0; i<N; i++) {
         const TooN::Vector<N> dxi = spread * L.T()[i];
         const TooN::Vector<M> yi1 = f(x + dxi);
         const TooN::Vector<M> yi2 = f(x - dxi);
         newx += w1 * (yi1 + yi2);
-        outer_product_upper_half<TooN::util::PlusEquals>(yi1, yi2, w1, newP);
+        Internal::outer_product_upper_half<TooN::util::PlusEquals>(yi1, yi2, 
w1, newP);
     }
-    outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
+    Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
     TooN::Symmetrize(newP);
 }
 
@@ -55,17 +58,17 @@
     static const double spread = sqrt(N/(1-w0));
     const TooN::Vector<M> y0 = f(x);
     newx = w0 * y0;
-    outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
+    Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
     TooN::Zero(cov);
     for (int i=0; i<N; i++) {
         const TooN::Vector<N> dxi = spread * L.T()[i];
         const TooN::Vector<M> yi1 = f(x + dxi);
         const TooN::Vector<M> yi2 = f(x - dxi);
         newx += w1 * (yi1 + yi2);
-        outer_product_upper_half<TooN::util::PlusEquals>(yi1, yi2, w1, newP);
-        add_product(w1 * (yi1-yi2).as_col(), dxi.as_row(), cov);
+        Internal::outer_product_upper_half<TooN::util::PlusEquals>(yi1, yi2, 
w1, newP);
+        TooN::add_product(w1 * (yi1-yi2).as_col(), dxi.as_row(), cov);
     }
-    outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
+    Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
     TooN::Symmetrize(newP);
 }
 
@@ -89,19 +92,19 @@
     }
     const TooN::Vector<M> y0 = f(x);
     newx = w0 * y0;
-    outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
+    Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
     for(int i = 0; i < N; i+=2){
         const TooN::Vector<M> res = f(x + L * xarg[i]);
         const TooN::Vector<M> res2 = f(x + L * xarg[i+1]);
         newx += w1 * (res+res2);
-        outer_product_upper_half<TooN::util::PlusEquals>(res, res2, w1, newP);
+        Internal::outer_product_upper_half<TooN::util::PlusEquals>(res, res2, 
w1, newP);
     }
     if(N % 2 == 0){
         const TooN::Vector<M> res = f(x + L * xarg[N]);
         newx += w1 * res;
-        outer_product_upper_half<TooN::util::PlusEquals>(res, w1, newP);
+        Internal::outer_product_upper_half<TooN::util::PlusEquals>(res, w1, 
newP);
     }
-    outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
+    Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
     TooN::Symmetrize(newP);
 }
 
@@ -126,19 +129,18 @@
     Zero(cov);
     const TooN::Vector<M> y0 = f(x);
     newx = w0 * y0;
-    outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
+    Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
     for(int i = 0; i < N+1; ++i){
         const TooN::Vector<N> d = L * xarg[i];
         const TooN::Vector<M> res = f(x + d);
         newx += w1 * res;
-        outer_product_upper_half<TooN::util::PlusEquals>(res, w1, newP);
-        add_product(w1 * (res - y0).as_col(), d.as_row(), cov);
+        Internal::outer_product_upper_half<TooN::util::PlusEquals>(res, w1, 
newP);
+        TooN::add_product(w1 * (res - y0).as_col(), d.as_row(), cov);
     }
-    outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
+    Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
     TooN::Symmetrize(newP);
 }
 
-}
 
 /// computes the unscented transform of the distribution given by @a x and @a 
P through the function @a f using
 /// 2*N + 1 sigma points distributed along the eigenvectors of the covariance 
matrix. Results are written into
@@ -154,7 +156,7 @@
 {
     TooN::Matrix<N> L;
     TooN::Cholesky<N>::sqrt(P,L);
-    Internal::unscentedTransformSqrt(x, L, f, newx, newP);
+    unscentedTransformSqrt(x, L, f, newx, newP);
 }
 
 /// computes the unscented transform of the distribution given by @a x and @a 
P through the function @a f using
@@ -172,7 +174,7 @@
 {
     TooN::Matrix<N> L;
     TooN::Cholesky<N>::sqrt(P,L);
-    Internal::unscentedTransformSqrt(x, L, f, newx, newP, cov);
+    unscentedTransformSqrt(x, L, f, newx, newP, cov);
 }
 
 /// computes the unscented transform of the distribution given by @a x and @a 
P through the function @a f using
@@ -187,7 +189,7 @@
 void sphericalUnscentedTransform(const TooN::FixedVector<N, V1>& x, const 
TooN::FixedMatrix<N,N,M1>& P, const F& f, TooN::FixedVector<M,V2>& newx, 
TooN::FixedMatrix<M,M,M2>& newP){
     TooN::Matrix<N> L;
     TooN::Cholesky<N>::sqrt(P,L);
-    Internal::sphericalUnscentedTransformSqrt(x, L, f, newx, newP);
+    sphericalUnscentedTransformSqrt(x, L, f, newx, newP);
 }
 
 /// computes the unscented transform of the distribution given by @a x and @a 
P through the function @a f using
@@ -204,7 +206,7 @@
 void sphericalUnscentedTransform(const TooN::FixedVector<N, V1>& x, const 
TooN::FixedMatrix<N,N,M1>& P, const F& f, TooN::FixedVector<M,V2>& newx, 
TooN::FixedMatrix<M,M,M2>& newP, TooN::FixedMatrix<M,N,M3>& cov){
     TooN::Matrix<N> L;
     TooN::Cholesky<N>::sqrt(P,L);
-    Internal::sphericalUnscentedTransformSqrt(x, L, f, newx, newP, cov);
+    sphericalUnscentedTransformSqrt(x, L, f, newx, newP, cov);
 }
 
 template <int N, int M, class Ax, class AP, class AR, class F>
@@ -214,7 +216,8 @@
     TooN::Matrix<M> Pyy;
     TooN::Matrix<M,N> Pyx;
     unscentedTransform(x, P, f, v, Pyy, Pyx);
-    const TooN::Matrix<M> Sinv = inverse(Pyy + R);
+    TooN::Cholesky<M> chol(Pyy + R);
+    const TooN::Matrix<M> Sinv = chol.get_inverse();
     x += Pyx.T() * (Sinv * v);
     P -= transformCovariance(Pyx.T(), Sinv);
 }