diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-07 00:05:29 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-07 00:05:29 +0200 |
commit | 5559e568b6e5502ad51ec96fd531876c1191d641 (patch) | |
tree | bc6356b5bed0481cced4a7d7e7ef99bd225d91a7 /src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m | |
parent | 863385dbc4243c8ecb19890a0dfce4a0b7ead9d6 (diff) | |
parent | d67089b23f58ac152253f58c5deaebbd57db0362 (diff) | |
download | px4-firmware-safelink.tar.gz px4-firmware-safelink.tar.bz2 px4-firmware-safelink.zip |
Merged master into safelinksafelink
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, 0 insertions, 26 deletions
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m deleted file mode 100755 index f94cce1fb..000000000 --- a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m +++ /dev/null @@ -1,26 +0,0 @@ -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 - |