From 482cada59ba87bc1ac538f7f165a57880e7fbeda Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 23 Mar 2013 22:39:54 +0100
Subject: Butchered position estimator from Damian Aregger into shape,
 publishes now global position estimate as well. Compiling, needs HIL testing

---
 .../positionKalmanFilter1D_dT.m                    | 26 ++++++++++++++++++++++
 1 file changed, 26 insertions(+)
 create mode 100755 apps/position_estimator_mc/positionKalmanFilter1D_dT.m

(limited to 'apps/position_estimator_mc/positionKalmanFilter1D_dT.m')

diff --git a/apps/position_estimator_mc/positionKalmanFilter1D_dT.m b/apps/position_estimator_mc/positionKalmanFilter1D_dT.m
new file mode 100755
index 000000000..f94cce1fb
--- /dev/null
+++ b/apps/position_estimator_mc/positionKalmanFilter1D_dT.m
@@ -0,0 +1,26 @@
+function  [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
+    %dynamics
+    A = [1 dT -0.5*dT*dT;
+         0 1 -dT;
+         0 0 1];
+    B = [0.5*dT*dT; dT; 0];
+    C = [1 0 0];
+    %prediction
+    x_apriori=A*x_aposteriori_k+B*u;
+    P_apriori=A*P_aposteriori_k*A'+Q;
+    if abs(u)<thresh
+        x_apriori(2)=decay*x_apriori(2);
+    end
+    %update
+    if gps_update==1
+        y=z-C*x_apriori;
+        S=C*P_apriori*C'+R;
+        K=(P_apriori*C')/S;
+        x_aposteriori=x_apriori+K*y;
+        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
+    else
+        x_aposteriori=x_apriori;
+        P_aposteriori=P_apriori;
+    end
+end
+
-- 
cgit v1.2.3