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/positionKalmanFilter1D_dT.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/positionKalmanFilter1D_dT.m')
-rwxr-xr-x | src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m | 26 |
1 files changed, 26 insertions, 0 deletions
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m new file mode 100755 index 000000000..f94cce1fb --- /dev/null +++ b/src/modules/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 + |