Covs calculates sigma points correctly and uses the right ones

Sinecamellia · Nov 11, 2014 · c5f93b0 · c5f93b0
1 parent 3ed936b
commit c5f93b0
Showing 1 changed file with 4 additions and 4 deletions.
diff --git a/src/augmented.jl b/src/augmented.jl
@@ -57,17 +57,17 @@ end
 
 function covs(kf::AugmentedUnscentedKalmanFilter,y::Observation)
     (x,p) = augment(kf.x,kf.z.r)
-    σn = sigma(x,p,kf.x.α,kf.x.κ)
+    (σn,wm,wc) = sigma(x,p,kf.x.α,kf.x.β,kf.x.κ)
     yp = map(kf.z.h,σn)
 
-    yhat = dot(kf.x.wm,yp)
+    yhat = dot(wm,yp)
 
     resx = map(x->x-kf.x.x,σn)
     resy = map(y->y-yhat,yp)
 
     res = y.y-yhat
-    ph = dot(kf.x.wc,map((x,z)->x*z',resx,resy))
-    s = dot(kf.x.wc,map(x->x*x',resy))
+    ph = dot(wc,map((x,z)->x*z',resx,resy))
+    s = dot(wc,map(x->x*x',resy))
 
     return res,ph,s
 end