diff options
| author | px4dev <px4@purgatory.org> | 2013-04-26 11:27:26 -0700 |
|---|---|---|
| committer | px4dev <px4@purgatory.org> | 2013-04-26 11:27:26 -0700 |
| commit | 3acdc9d4ce3d83af6bb7f953e466620be690658e (patch) | |
| tree | c1c792b44f7454ce36a288034179764ebc355daa /apps/position_estimator_mc/positionKalmanFilter1D.m | |
| parent | 8224adf52a126105c72e41db2ba35b1aaed3e301 (diff) | |
| parent | 556a017444b809c18e2ce495a2fd00380960e0f4 (diff) | |
| download | px4-firmware-3acdc9d4ce3d83af6bb7f953e466620be690658e.tar.gz px4-firmware-3acdc9d4ce3d83af6bb7f953e466620be690658e.tar.bz2 px4-firmware-3acdc9d4ce3d83af6bb7f953e466620be690658e.zip | |
Merge branch 'master' into export-build
Diffstat (limited to 'apps/position_estimator_mc/positionKalmanFilter1D.m')
| -rwxr-xr-x | apps/position_estimator_mc/positionKalmanFilter1D.m | 19 |
1 files changed, 19 insertions, 0 deletions
diff --git a/apps/position_estimator_mc/positionKalmanFilter1D.m b/apps/position_estimator_mc/positionKalmanFilter1D.m new file mode 100755 index 000000000..144ff8c7c --- /dev/null +++ b/apps/position_estimator_mc/positionKalmanFilter1D.m @@ -0,0 +1,19 @@ +function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) +%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 |