blob: 144ff8c7c2b92faaa4d9ea2c6c5141a6ae895760 (
plain) (
blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
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
|
