%%HP: T(3)A(D)F(.);
@ by Tom Metcalf
DIR
SOLVE
\<< -22 SF 4 FIX
DEG 0 0 0 0 0 GSUM
a0 \->NUM 'A0' STO a1
\->NUM 'A1' STO EV1
\->NUM DUP '\Ga1' STO
EIGEN 'E1' STO EV3
\->NUM DUP '\Ga3' STO
EIGEN 'E3' STO EV2
\->NUM DUP '\Ga2' STO
EIGEN 'E2' STO R E1
DOT '\Gb1' STO R E2
DOT '\Gb2' STO R E3
DOT '\Gb3' STO 0
'NIT' STO 0 '\Gm' STO
DO \Gm 'OLD'
STO ITER '\Gm' STO 1
'NIT' STO+
UNTIL 'ABS((\Gm
-OLD)/\Gm)<.000001 OR
NIT>50'
END
IF 'NIT>50 OR
\Gm >\Ga1'
THEN
"CONVERGENCE ERROR"
END UVW OBJ\->
DROP OUT
\>>
ADDOB
\<< \-> T A
\<< T HMS\-> 'T'
STO A HMS\-> 'A' STO
OBS
IFERR OBJ\->
THEN T GHA1
GHA2 INTERP T DEC1
DEC2 INTERP A { 1 3
} \->ARRY SWAP STO
ELSE OBJ\->
ROT 1 + ROT ROT
\->LIST T GHA1 GHA2
INTERP SWAP T DEC1
DEC2 INTERP SWAP A
SWAP \->ARRY 'OBS'
STO
END
\>>
\>>
CORRECT
\<< DEG HMS\-> INDX
+ HGT \v/ .0293 * -
DUP DUP DUP 4.4 +
7.31 SWAP / + TAN
.0167 SWAP / SWAP
COS
CASE BODY 'S'
SAME
THEN .0024
* SEMI
END BODY
'M' SAME
THEN HP *
HP .2724 *
END BODY
'VM' SAME
THEN HP * 0
END 0 * 0
END LU * +
SWAP - +
IF 'SPD>0'
THEN SWAP
HMS\-> DUP DUP GHA1
GHA2 INTERP SWAP
DEC1 DEC2 INTERP
SWAP DRLAT DRLON
AZIM DUP CSCORR ROT
SWAP - SWAP \->HMS
SWAP
END \->HMS
\>>
SETUP
\<< "BODY?" { ""
\Ga V } INPUT OBJ\->
'BODY' STO
"INDEX? (Deg)" { ""
V } INPUT OBJ\-> HMS\->
'INDX' STO
IF BODY 'S'
SAME
THEN
"SEMI-D? (Deg)" {
"" V } INPUT OBJ\->
HMS\-> 'SEMI' STO
END
IF BODY 'M'
SAME BODY 'VM' SAME
OR
THEN
"HP? (Deg)" { "" V
} INPUT OBJ\-> HMS\->
'HP' STO
END
IF BODY 'M'
SAME BODY 'S' SAME
OR
THEN
"LIMB (L/U/C=1/-1/0)?"
{ "" V } INPUT OBJ\->
'LU' STO
END
"HEIGHT (m)?" { ""
V } INPUT OBJ\->
'HGT' STO
"GHA1 DEC1 TIM1?" {
":GHA1:
:DEC1:
:TIM1:"
{ 1 0 } V } INPUT
OBJ\-> HMS\-> 'T1' STO
HMS\-> 'DEC1' STO
HMS\-> 'GHA1' STO
"GHA2 DEC2 TIM2" {
":GHA2:
:DEC2:
:TIM2:"
{ 1 0 } V } INPUT
OBJ\-> HMS\-> 'T2' STO
HMS\-> 'DEC2' STO
HMS\-> 'GHA2' STO
"SPEED? (Knots)" {
"" V } INPUT OBJ\->
'SPD' STO
IF 'SPD\=/0'
THEN
"COURSE? (True)" {
"" V } INPUT OBJ\->
HMS\-> 'CRS' STO
"DR LAT LON?" {
":LAT:
:LON:" { 1 0
} V } INPUT OBJ\->
HMS\-> 'DRLON' STO
HMS\-> 'DRLAT' STO
"TIME OF FIX?" { ""
V } INPUT OBJ\-> HMS\->
'TF' STO
ELSE 0 'CRS'
STO 0 'DRLAT' STO 0
'DRLON' STO 0 'TF'
STO
END
\>>
ERROR
\<< 0 0 0 0 0 0 0
0 \-> H1 H2 D1 D2 G1
G2 DT DH
\<< OBS { 1 3 }
GET 'H1' STO OBS {
N 3 } GET 'H2' STO
OBS { 1 2 } GET
'D1' STO OBS { N 2
} GET 'D2' STO OBS
{ 1 1 } GET 'G1'
STO OBS { N 1 } GET
'G2' STO T2 T1 -
GHA2 GHA1 - / G2 G1
- * 'DT' STO H2 H1
- 'DH' STO 1 DT / N
\v/ / 57.3 H1 H2 + 2
/ COS * * 225 D1 D2
+ 2 / COS SQ * DH
DT / SQ - \v/ / "ERR"
\->TAG
\>>
\>>
NIT 4
ITER
\<< 0 0 \-> f fp
\<< \Gb1 \Ga1 \Gm - /
SQ DUP 'f' STO+ 2 *
\Ga1 \Gm - / 'fp' STO+
\Gb2 \Ga2 \Gm - / SQ DUP
'f' STO+ 2 * \Ga2 \Gm -
/ 'fp' STO+ \Gb3 \Ga3 \Gm
- / SQ DUP 'f' STO+
2 * \Ga3 \Gm - / 'fp'
STO+ -1 'f' STO+ \Gm
f fp / -
\>>
\>>
a0 '-(G12*G23-G13
*G22)*G13+(G11*G23-
G12*G13)*G23-(G11*
G22-G12^2)*G33'
a1 'G11*G22-G12^2
+G11*G33-G13^2+G22*
G33-G23^2'
TF 0
DRLON 0
DRLAT 0
CRS 0
SPD 0
CSCORR
\<< \-> T
\<< SPD T TF -
AZ CRS - COS 60 / *
*
\>>
\>>
AZ 239.148905272
AZIM
\<< \-> D G L A
\<< G A - 'A'
STO L COS D SIN * L
SIN D COS A COS * *
- A SIN D COS NEG *
R\->C ARG 'AZ' STO
IF 'AZ<0'
THEN 360
'AZ' STO+
END
\>>
\>>
EV3 '-2*\v/Q*COS((\Gh
+360)/3)+N/3'
EV2 'N-\Ga1-\Ga3'
EV1 '-2*\v/Q*COS(\Gh/
3)+N/3'
OLD
-1.47280528459E-7
\Gm
-1.47296963855E-7
\Gb3 -13.5624809912
\Gb2
-1.50525051351E-2
\Gb1
-3.950284015E-7
E3
[ .188406852706 .980097318647 6.25468131232E-2 ]
E2
[ -1.40179729991E-2 6.63646826456E-2 -.997696960669 ]
E1
[ -.981991016574 .187096170084 2.62424562793E-2 ]
INTERP
\<< \-> T V1 V2
\<< V1 V2 V1 -
T2 T1 - / T T1 - *
+
\>>
\>>
GSUM
\<< \-> DS DC GS GC
HS
\<< 0 'G11' STO
0 'G12' STO 0 'G13'
STO 0 'G22' STO 0
'G23' STO { 3 } 0
CON 'R' STO OBS
OBJ\-> OBJ\-> DROP DROP
'N' STO 1 N
START SIN
'HS' STO DUP SIN
'DS' STO COS 'DC'
STO DUP SIN 'GS'
STO COS 'GC' STO DS
SQ 'G11' STO+ DS DC
GC * * 'G12' STO+
DS DC GS * * 'G13'
STO+ DC SQ GC SQ *
'G22' STO+ DC SQ GS
GC * * 'G23' STO+ R
OBJ\-> DROP DC GS HS
* * + ROT DS HS * +
ROT DC GC HS * * +
ROT { 3 } \->ARRY 'R'
STO
NEXT N G11
G22 + - 'G33' STO
\>>
\>>
OUT
\<< \-> U V W
\<<
IF 'ABS(U)>
1'
THEN U SIGN
'U' STO
END U ASIN
V W R\->C ARG \->HMS
"LON" \->TAG SWAP
\->HMS "LAT" \->TAG
\>>
\>>
UVW
\<< \Gb1 \Ga1 \Gm - /
E1 * \Gb2 \Ga2 \Gm - / E2
* \Gb3 \Ga3 \Gm - / E3 *
+ +
\>>
EIGEN
\<< \-> EV
\<< 'G12*G23-
G13*G22+G13*EV'
\->NUM 'G13*G12-G11*
G23+G23*EV' \->NUM '
G11*G22-SQ(G12)-(
G11+G22)*EV+SQ(EV)'
\->NUM { 3 } \->ARRY
DUP ABS /
\>>
\>>
\Ga2 .0363326349
\Ga3 17.9636667352
\Ga1 .0000006299
\Gh 'ACOS(R1/Q^1.5)
'
R1 'A0/2+N/3*(A1/
6-Q)'
Q '(N/3)^2-A1/3'
N 18
A0
-4.110603687E-7
A1 .6526786832
G33 .1064412066
R
[ -2.55505296373 -13.2935502826 -.833272135648 ]
G23 1.09880240075
G22 17.255892215
G13 .212196428198
G12 3.31708381131
G11 .637666578414
GHA2
92.6916666667
DEC2
16.4916666667
T2 18
GHA1 77.65
DEC1
16.4916666667
T1 17
LU 1
SEMI
.266666666667
HP .986666666667
HGT 0
INDX 0
BODY T
END
