-
Notifications
You must be signed in to change notification settings - Fork 4
/
BPlaneCompute.m
45 lines (30 loc) · 984 Bytes
/
BPlaneCompute.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
36
37
38
39
40
41
42
43
44
45
function BPlaneStuff = BPlaneCompute(state, mu)
rVec = state(1:3);
vVec = state(4:6);
r = norm(rVec);
v = norm(vVec);
eVec = 1/mu*((v^2 - mu/r)*rVec - dot(rVec,vVec)*vVec);
e = norm(eVec);
PHat = eVec/e;
hVec = cross(rVec, vVec);
h = norm(hVec);
WHat = hVec/h;
QHat = cross(WHat, PHat);
p = h^2/mu;
%a = p/(1-e);
a = 1/(2/r - v^2/mu);
b = abs(a)*sqrt(e^2 - 1);
SHat = vVec/v;
NHat = [0; 0; 1];
THat = cross(SHat, NHat)/norm(cross(SHat, NHat));
RHat = cross(SHat, THat);
s = -dot(rVec, vVec)/v^2;
B = b*cross(SHat, WHat);
%B = rVec + s*vVec;
% Find rotation matrix from ECI frame to B-Plane frame (S, T, R)
BPlaneDCM = [SHat, THat, RHat]';
nu = acos(dot(rVec/r, PHat));
f = acosh(1 + v^2/mu*a*(1-e^2)/(1+e*cos(nu)));
LTOF = mu/v^3*((sinh(f) - f));
BPlaneStuff = {SHat, THat, RHat, BPlaneDCM, B, LTOF};
end