TooN Algorithm Library - tag  0.2
unscented.h
Go to the documentation of this file.
1 #ifndef TAG_UNSCENTED_H
2 #define TAG_UNSCENTED_H
3 
4 #include <TooN/Cholesky.h>
5 #include <TooN/helpers.h>
6 
7 namespace tag {
8 
14 
15 namespace Internal {
16 
17 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)
18 {
19  for (int i=0; i<N; ++i)
20  for (int j=i; j<N; ++j)
21  F::eval(m[i][j], w * v[i] * v[j]);
22 }
23 
24 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)
25 {
26  for (int i=0; i<N; ++i)
27  for (int j=i; j<N; ++j)
28  F::eval(m[i][j], w * (v1[i] * v1[j] + v2[i] * v2[j]));
29 }
30 
31 }
32 
33 template <int N, int M, class F, class V1, class V2, class M1, class M2>
34 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 {
36  static const double w0 = 1/3.0;
37  static const double w1 = (1-w0)/(2 * N);
38  static const double spread = sqrt(N/(1-w0));
39  const TooN::Vector<M> y0 = f(x);
40  newx = w0 * y0;
41  Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
42  for (int i=0; i<N; i++) {
43  const TooN::Vector<N> dxi = spread * L.T()[i];
44  const TooN::Vector<M> yi1 = f(x + dxi);
45  const TooN::Vector<M> yi2 = f(x - dxi);
46  newx += w1 * (yi1 + yi2);
47  Internal::outer_product_upper_half<TooN::util::PlusEquals>(yi1, yi2, w1, newP);
48  }
49  Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
50  TooN::Symmetrize(newP);
51 }
52 
53 template <int N, int M, class F, class V1, class V2, class M1, class M2, class M3>
54 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, TooN::FixedMatrix<M,N,M3>& cov)
55 {
56  static const double w0 = 1/3.0;
57  static const double w1 = (1-w0)/(2 * N);
58  static const double spread = sqrt(N/(1-w0));
59  const TooN::Vector<M> y0 = f(x);
60  newx = w0 * y0;
61  Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
62  TooN::Zero(cov);
63  for (int i=0; i<N; i++) {
64  const TooN::Vector<N> dxi = spread * L.T()[i];
65  const TooN::Vector<M> yi1 = f(x + dxi);
66  const TooN::Vector<M> yi2 = f(x - dxi);
67  newx += w1 * (yi1 + yi2);
68  Internal::outer_product_upper_half<TooN::util::PlusEquals>(yi1, yi2, w1, newP);
69  TooN::add_product(w1 * (yi1-yi2).as_col(), dxi.as_row(), cov);
70  }
71  Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
72  TooN::Symmetrize(newP);
73 }
74 
75 template<int N, int M, class F, class V1, class V2, class M1, class M2>
76 void sphericalUnscentedTransformSqrt(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){
77  static const double w0 = 1/3.0;
78  static const double w1 = (1 - w0)/(N+1);
79  static TooN::Matrix<N+1, N> xarg;
80  static bool init = false;
81  if(!init){
82  init = true;
83  Zero(xarg);
84  for(int i = 0; i < N; ++i)
85  xarg[0][i] = xarg[1][i] = -1.0/sqrt((i+1)*(i+2)*w1);
86  xarg(0,0) *= -1.0;
87  for(int i = 1; i < N; ++i){
88  xarg[i+1][i] = -(i+1)*xarg[i][i];
89  for(int j = i+1; j < N; ++j)
90  xarg[i+1][j] = xarg[i][j];
91  }
92  }
93  const TooN::Vector<M> y0 = f(x);
94  newx = w0 * y0;
95  Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
96  for(int i = 0; i < N; i+=2){
97  const TooN::Vector<M> res = f(x + L * xarg[i]);
98  const TooN::Vector<M> res2 = f(x + L * xarg[i+1]);
99  newx += w1 * (res+res2);
100  Internal::outer_product_upper_half<TooN::util::PlusEquals>(res, res2, w1, newP);
101  }
102  if(N % 2 == 0){
103  const TooN::Vector<M> res = f(x + L * xarg[N]);
104  newx += w1 * res;
105  Internal::outer_product_upper_half<TooN::util::PlusEquals>(res, w1, newP);
106  }
107  Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
108  TooN::Symmetrize(newP);
109 }
110 
111 template<int N, int M, class F, class V1, class V2, class M1, class M2, class M3>
112 void sphericalUnscentedTransformSqrt(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, TooN::FixedMatrix<M,N,M3>& cov ){
113  static const double w0 = 1/3.0;
114  static const double w1 = (1 - w0)/(N+1);
115  static TooN::Matrix<N+1, N> xarg;
116  static bool init = false;
117  if(!init){
118  init = true;
119  Zero(xarg);
120  for(int i = 0; i < N; ++i)
121  xarg[0][i] = xarg[1][i] = -1.0/sqrt((i+1)*(i+2)*w1);
122  xarg(0,0) *= -1.0;
123  for(int i = 1; i < N; ++i){
124  xarg[i+1][i] = -(i+1)*xarg[i][i];
125  for(int j = i+1; j < N; ++j)
126  xarg[i+1][j] = xarg[i][j];
127  }
128  }
129  Zero(cov);
130  const TooN::Vector<M> y0 = f(x);
131  newx = w0 * y0;
132  Internal::outer_product_upper_half<TooN::util::Assign>(y0, w0, newP);
133  for(int i = 0; i < N+1; ++i){
134  const TooN::Vector<N> d = L * xarg[i];
135  const TooN::Vector<M> res = f(x + d);
136  newx += w1 * res;
137  Internal::outer_product_upper_half<TooN::util::PlusEquals>(res, w1, newP);
138  TooN::add_product(w1 * (res - y0).as_col(), d.as_row(), cov);
139  }
140  Internal::outer_product_upper_half<TooN::util::PlusEquals>(newx, -1, newP);
141  TooN::Symmetrize(newP);
142 }
143 
144 
154 template <int N, int M, class F, class V1, class V2, class M1, class M2>
155 void unscentedTransform(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)
156 {
157  TooN::Matrix<N> L;
158  TooN::Cholesky<N>::sqrt(P,L);
159  unscentedTransformSqrt(x, L, f, newx, newP);
160 }
161 
172 template <int N, int M, class F, class V1, class V2, class M1, class M2, class M3>
173 void unscentedTransform(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)
174 {
175  TooN::Matrix<N> L;
176  TooN::Cholesky<N>::sqrt(P,L);
177  unscentedTransformSqrt(x, L, f, newx, newP, cov);
178 }
179 
188 template <int N, int M, class F, class V1, class V2, class M1, class M2>
189 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){
190  TooN::Matrix<N> L;
191  TooN::Cholesky<N>::sqrt(P,L);
192  sphericalUnscentedTransformSqrt(x, L, f, newx, newP);
193 }
194 
205 template <int N, int M, class F, class V1, class V2, class M1, class M2, class M3>
206 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){
207  TooN::Matrix<N> L;
208  TooN::Cholesky<N>::sqrt(P,L);
209  sphericalUnscentedTransformSqrt(x, L, f, newx, newP, cov);
210 }
211 
212 template <int N, int M, class Ax, class AP, class AR, class F>
213 void unscentedKalmanUpdate(TooN::FixedVector<N,Ax>& x, TooN::FixedMatrix<N,N,AP>& P, const F& f, const TooN::FixedMatrix<M,M,AR>& R)
214 {
215  TooN::Vector<M> v;
216  TooN::Matrix<M> Pyy;
217  TooN::Matrix<M,N> Pyx;
218  unscentedTransform(x, P, f, v, Pyy, Pyx);
219  TooN::Cholesky<M> chol(Pyy + R);
220  const TooN::Matrix<M> Sinv = chol.get_inverse();
221  x += Pyx.T() * (Sinv * v);
222  P -= transformCovariance(Pyx.T(), Sinv);
223 }
224 
225 }
226 
227 #endif
Generated on Tue Aug 13 2013 14:11:40 for TooN Algorithm Library - tag by   doxygen 1.8.3.1