diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 12:59:47 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 12:59:47 +0200 |
commit | e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629 (patch) | |
tree | 1ae45990d97e3bd1d65574380ae1eeab5e41ff07 /src/modules/position_estimator_mc/kalman_dlqe3.m | |
parent | 574e76532126fea8ab0ac5fd0595f6fb2935f0dd (diff) | |
download | px4-firmware-e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629.tar.gz px4-firmware-e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629.tar.bz2 px4-firmware-e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629.zip |
Moved position_estimator_mc, px4io driver and sdlog app to new style build
Diffstat (limited to 'src/modules/position_estimator_mc/kalman_dlqe3.m')
-rwxr-xr-x | src/modules/position_estimator_mc/kalman_dlqe3.m | 17 |
1 files changed, 17 insertions, 0 deletions
diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m new file mode 100755 index 000000000..4c6421b7f --- /dev/null +++ b/src/modules/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 |