From a6a6a899dbc921ae72aed4c2b0cb7b5859a179ba Mon Sep 17 00:00:00 2001
From: Kevin0602 <33149101+Kevin0602@users.noreply.github.com>
Date: Thu, 14 Dec 2017 17:01:27 +0800
Subject: [PATCH] Add files via upload

The formula calculating  state covariance of previous step and estimated covariance of y missed the Q and R to plus
---
 ukf_predict3.m | 5 ++---
 ukf_update3.m  | 8 +++++---
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/ukf_predict3.m b/ukf_predict3.m
index b3b65d0..12dc71d 100644
--- a/ukf_predict3.m
+++ b/ukf_predict3.m
@@ -38,8 +38,7 @@
 %   UKF_PREDICT1, UKF_UPDATE1, UKF_PREDICT2, UKF_UPDATE2, UKF_UPDATE3
 %   UT_TRANSFORM, UT_WEIGHTS, UT_MWEIGHTS, UT_SIGMAS 
 
-% Copyright (C) 2003-2006 Simo S�rkk�
-% Copyright (C) 2007 Jouni Hartikainen
+% Copyright (C) 2003-2006 Simo S�rkk�% Copyright (C) 2007 Jouni Hartikainen
 %
 % $Id: ukf_predict3.m 480 2010-10-18 07:45:48Z jmjharti $
 %
@@ -101,7 +100,7 @@
   
   tr_param = {alpha beta kappa mat};
   [M,P,C,X_s,X_pred,w] = ut_transform(MA,PA,f,f_param,tr_param);
-    
+  P=P+Q;
   % Save sigma points
   X = X_s;
   X(1:size(X_pred,1),:) = X_pred;
diff --git a/ukf_update3.m b/ukf_update3.m
index feca1d8..6393066 100644
--- a/ukf_update3.m
+++ b/ukf_update3.m
@@ -58,8 +58,7 @@
 % References:
 %   [1] Wan, Merwe: The Unscented Kalman Filter
 %
-% Copyright (C) 2007 Jouni Hartikainen, Simo S�rkk�
-%
+% Copyright (C) 2007 Jouni Hartikainen, Simo S�rkk�%
 % $Id: ukf_update3.m 480 2010-10-18 07:45:48Z jmjharti $
 %
 % This software is distributed under the GNU General Public 
@@ -102,9 +101,12 @@
   %
   tr_param = {alpha beta kappa mat X w};
   [MU,S,C,X,Y_s] = ut_transform(M,P,h,h_param,tr_param);
+  S=S+R;
   K = C / S;
   M = M + K * (Y - MU);
-  P = P - K * S * K';
+  A=chol(S);
+  P = P - (K * A) *( K*A)';
+
   if nargout > 5
     LH = gauss_pdf(Y,MU,S);
   end
\ No newline at end of file