1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
|
function [xapo1,Papo1] = position_estimator(u,z,xapo,Papo,gps_covariance,predict_only) %if predit_onli == 1: no update step: use this when no new gps data is available
%#codegen
%%initialization
%use model F=m*a x''=F/m
% 250Hz---> dT = 0.004s
%u=[phi;theta]
%x=[px;vx;py;vy];
%%------------------------------------------
dT=0.004;
%%------------------------------------------------
%R_t=[1,-r*dT,q*dT;r*dT,1,-p*dT;-q*dT,p*dT,1];
F=[ 1, 0.004, 0, 0, 0, 0;
0, 1, 0, 0, 0, 0;
0, 0, 1, 0.004, 0, 0;
0, 0, 0, 1, 0, 0;
0, 0, 0, 0, 1, 0.004;
0, 0, 0, 0, 0, 1];
B=[ 0, -0.1744;
0, -87.2;
0.1744, 0;
87.2, 0;
0, 0;
0, 0];
H=[1,0,0,0,0,0;
0,0,1,0,0,0;
0,0,0,0,1,0];
Q=[1e-007 ,0 ,0 ,0 ,0 ,0;
0 ,1 ,0 ,0 ,0 ,0;
0 ,0 ,1e-007 ,0 ,0 ,0;
0 ,0 ,0 ,1 ,0 ,0
0 ,0 ,0 ,0 ,1e-007 ,0;
0 ,0 ,0 ,0 ,0 ,1]; %process Covariance Matrix
R=[gps_covariance(1), 0, 0;
0, gps_covariance(2), 0;
0, 0, gps_covariance(3)]; %measurement Covariance Matrix
%%prediction
xapri=F*xapo+B*u;
Papri=F*Papo*F'+Q;
if 1 ~= predict_only
%update
yr=z-H*xapri;
S=H*Papri*H'+R;
K=(Papri*H')/S;
xapo1=xapri+K*yr;
Papo1=(eye(6)-K*H)*Papri;
else
Papo1=Papri;
xapo1=xapri;
end
|
