From e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 12:59:47 +0200 Subject: Moved position_estimator_mc, px4io driver and sdlog app to new style build --- src/modules/position_estimator_mc/kalman_dlqe2.m | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100755 src/modules/position_estimator_mc/kalman_dlqe2.m (limited to 'src/modules/position_estimator_mc/kalman_dlqe2.m') diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m new file mode 100755 index 000000000..2a50164ef --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe2.m @@ -0,0 +1,9 @@ +function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z) + st = 1/2*dt^2; + A = [1,dt,st; + 0,1,dt; + 0,0,1]; + C=[1,0,0]; + K = [k1;k2;k3]; + x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); +end \ No newline at end of file -- cgit v1.2.3