[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[Toon-members] tag/tag unscented.h
From: |
Gerhard Reitmayr |
Subject: |
[Toon-members] tag/tag unscented.h |
Date: |
Tue, 11 Dec 2007 15:41:08 +0000 |
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);
}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [Toon-members] tag/tag unscented.h,
Gerhard Reitmayr <=