1 files changed, 17 insertions, 0 deletions
diff --git a/apps/position_estimator_mc/kalman_dlqe3.m b/apps/position_estimator_mc/kalman_dlqe3.m
new file mode 100755
index 000000000..4c6421b7f
--- /dev/null
+++ b/apps/position_estimator_mc/kalman_dlqe3.m
@@ -0,0 +1,17 @@
+function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)
+    st = 1/2*dt^2;
+    A = [1,dt,st;
+        0,1,dt;
+        0,0,1];
+    C=[1,0,0];
+    K = [k1;k2;k3];
+    if addNoise==1
+        noise = sigma*randn(1,1);
+        z = z + noise;
+    end
+    if(posUpdate)
+        x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);
+    else
+        x_aposteriori=A*x_aposteriori_k;
+    end
+end
+\ No newline at end of file