-
Notifications
You must be signed in to change notification settings - Fork 5
/
KalmanPhi_T.m
35 lines (35 loc) · 1.82 KB
/
KalmanPhi_T.m
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
function Ft = KalmanPhi_T(Vn,Cnb,Pos,Fn,Rm,Rn)
glvs;
F11=[(Vn(2)*tan(Pos(1))-Vn(3))/(Rn+Pos(3)),2*glv.Wie*sin(Pos(1))+(Vn(1)*tan(Pos(1)))/(Rn+Pos(3)),-2*glv.Wie*cos(Pos(1))-Vn(1)/(Rn+Pos(3));
-2*glv.Wie*sin(Pos(1))-2*(Vn(1)*tan(Pos(1)))/(Rn+Pos(3)),-Vn(3)/(Rm+Pos(3)),-Vn(2)/(Rm+Pos(3));
2*glv.Wie*cos(Pos(1))+2*Vn(1)/(Rn+Pos(3)),2*Vn(2)/(Rm+Pos(3)),0 ];
F12=[0,-Fn(3),Fn(2);
Fn(3),0,-Fn(1);
-Fn(2),Fn(1),0 ];
F13=[2*glv.Wie*(Vn(3)*sin(Pos(1))+Vn(2)*cos(Pos(1)))+Vn(1)*Vn(2)*sec(Pos(1))^2/(Rn+Pos(3)) 0 (Vn(1)*Vn(3)-Vn(1)*Vn(2)*tan(Pos(1)))/(Rn+Pos(3))^2;
-2*Vn(1)*glv.Wie*cos(Pos(1))-Vn(1)^2*sec(Pos(1))^2/(Rn+Pos(3)) 0 Vn(2)*Vn(3)/(Rm+Pos(3))^2+Vn(1)^2*tan(Pos(1))/(Rn+Pos(3))^2;
-2*Vn(1)*glv.Wie*sin(Pos(1)) 0 -Vn(2)^2/(Rm+Pos(3))^2-Vn(1)^2/(Rn+Pos(3))^2];
F14=Cnb;
F21=[0,-1/(Rm+Pos(3)),0;
1/(Rn+Pos(3)),0,0;
tan(Pos(1))/(Rn+Pos(3)),0,0];
F22=[0,glv.Wie*sin(Pos(1))+Vn(1)*tan(Pos(1))/(Rn+Pos(3)),-glv.Wie*cos(Pos(1))-Vn(1)/(Rn+Pos(3));
-glv.Wie*sin(Pos(1))-Vn(1)*tan(Pos(1))/(Rn+Pos(3)),0,-Vn(2)/(Rm+Pos(3));
glv.Wie*cos(Pos(1))+Vn(1)/(Rn+Pos(3)),Vn(2)/(Rm+Pos(3)),0];
F23=[0 0 Vn(2)/(Rm+Pos(3))^2;
-glv.Wie*sin(Pos(1)) 0 -Vn(1)/(Rn+Pos(3))^2;
glv.Wie*cos(Pos(1))+Vn(1)*sec(Pos(1))^2/(Rn+Pos(3)) 0 -Vn(1)*tan(Pos(1))/(Rn+Pos(3))^2];
F25=-Cnb;
F31=[0 1/(Rm+Pos(3)) 0;
sec(Pos(1))/(Rn+Pos(3)) 0 0;
0 0 1 ];
F33=[0 0 -Vn(2)/(Rm+Pos(3))^2;
Vn(1)*tan(Pos(1))*sec(Pos(1))/(Rn+Pos(3)) 0 -Vn(1)*sec(Pos(1))/(Rn+Pos(3))^2;
0 0 0 ];
Ft=[ F11,F12,F13,F14,zeros(3,5);
F21,F22,F23,zeros(3,3),F25,zeros(3,2);
F31,zeros(3,3),F33,zeros(3,6),zeros(3,2);
zeros(6,17);
zeros(1,16),1;
zeros(1,17)];
end