with Calendar; use Calendar; package body B000003a is -- Declare the necessary types and subtypes type Real is digits 9 range -1.0E+9 .. 1.0E+9; subtype Real14 is Real range -16383.0 .. 16383.0; subtype Real12 is Real range -4095.0 .. 4095.0; subtype Real11 is Real range -2047.0 .. 2047.0; subtype Real10 is Real range -1023.0 .. 1023.0; subtype Real9 is Real range -511.0 .. 511.0; subtype Real5 is Real range -127.0 .. 127.0; subtype Real8u is Real range 0.0 .. 255.0; subtype Real6u is Real range 0.0 .. 63.0; subtype Real4u is Real range 0.0 .. 15.0; subtype Real3u is Real range 0.0 .. 7.0; subtype Real1u is Real range 0.0 .. 2.0; subtype Fract is Real range -1.0 .. 1.0; subtype Fractu is Real range 0.0 .. 1.0; subtype Fractn is Real range -1.0 .. 0.0; subtype Fract5u is Real range 0.0 .. 0.0312; subtype Fract17u2 is Real range 0.0 .. 0.762E-5; subtype Fract13u2 is Real range 0.0 .. 0.122E-3; subtype Fract12u2 is Real range 0.0 .. 0.244E-3; type Afract17u2 is array (1 .. 2) of Fract17u2; type Afract13u2 is array (1 .. 2) of Fract13u2; type Afract12u2 is array (1 .. 2) of Fract12u2; type Afractu is array (1 .. 3) of Fractu; type Afractu2 is array (1 .. 2) of Fractu; type Afractu8 is array (1 .. 8) of Fractu; type Areal3u is array (1 .. 2) of Real3u; type Areal1u is array (1 .. 11) of Real1u; type Areal6u is array (1 .. 4) of Real6u; subtype Int18u is Real range 0.0 .. 262143.0; subtype Int16u is Real range 0.0 .. 65535.0; subtype Int15u is Real range 0.0 .. 32767.0; subtype Int14u is Real range 0.0 .. 16383.0; subtype Int12s is Real range -2047.0 .. 2047.0; subtype Int3u is Real range 0.0 .. 7.0; subtype Int3s is Real range -3.0 .. 3.0; type Aint3u50 is array (1 .. 3) of Int3u; type Aint15u50 is array (1 .. 3) of Int15u; type Aint15u11 is array (1 .. 11) of Int15u; type Aint15u4 is array (1 .. 4) of Int15u; type Aint16u50 is array (1 .. 3) of Int16u; type Aint18u4 is array (1 .. 4) of Int18u; -- give some variables initial values Iivmi2 : Fractu := 0.34619; Iivmj2 : Fractn := -0.04711; Iivmk2 : Fractu := 0.93698; Iivti2 : Fractu := 0.34198; Iivtj2 : Fractu := 0.041294; Iivtk2 : Fractn := -0.93959; Ijai : Fractn := -0.67405; Ijaj : Fractu := 0.70539; Ijak : Fractu := 0.21923; Ijapi : Fractn := -0.653465; Ijapj : Fractu := 0.705007; Ijapk : Fractu := 0.275583; Ikai : Fractn := -0.69815; Ikaj : Fractu := 0.70532; Ikak : Fractu := 0.12287; Ikapi : Fractn := -0.664248; Ikapj : Fractn := -0.70866; Ikapk : Fractu := 0.237857; Inpua : Real11 := -17.12; Inyua : Real11 := 5.98; Rts : Afractu := (0.1, 0.1, 0.1); Ivmi2, Ivmk2, Ivti2, Ivtj2, Jaj, Jak, Japj, Japk, Kaj, Kak, Kapk : Fractu; Ivmj2, Ivtk2, Jai, Japi, Kai, Kapi, Kapj : Fractn; Npua, Nyua : Real11; Ts : Fractu; K1, K2, K3 : Real12; Mp_11, Mp_21, Mp11, Mp21 : Real10; Mp_22, Mp_31, Mp_32, Mp_33, Mp22, Mp31, Mp32, Mp33 : Real11; N1ps, N1ys, Ypdh, Yydh, Nnpc, Nnyc : Real5; Q21, Q31, Q22, Q32 : Afractu2; Rdt : Fractu; Q33 : Areal3u; Betai : Real4u; S2s : Fract5u; Phi13 : Afractu2 := (0.005, 0.00125); Phi33 : Afractu2 := (0.98, 0.99); Q32p : Afractu2 := (0.002, 0.0005); Q33p : Afractu2 := (0.04, 0.02); Q21p : Afract17u2 := (0.000005, 0.0000003125); Q31p : Afract13u2 := (0.0000667, 0.00000833); Q22p : Afract12u2 := (0.000133, 0.0000167); S2fn : Fractu := 0.1E-5; S2he : Fractu := 0.1225E-2; S2rno : Fractu := 0.9216E-2; S2sno : Fractu := 0.76E-4; Cc35 : Afractu8 := (0.0, 0.67032, 0.76874, 0.72470, 0.71677, 0.8114, 0.80092, 0.89494); Cc34 : Afractu8 := (0.0, 0.40254, 0.27804, 0.36784, 0.39511, 0.24238, 0.27773, 0.14656); R_Mto : Real8u := 104.8; D12 : Fractu := 0.73539; D13 : Fractu := 0.15205; D23 : Fractu := 0.46802; Rd : Areal1u := (1.225, 0.963, 0.7463, 0.5691, 0.4262, 0.3027, 0.2064, 0.1407, 0.0961, 0.0656, 0.0407); Beta : Areal6u := (0.0, 41.636, 41.636, 41.636); Realt : Real; Half : constant Real := 0.5; C05 : constant Real := 0.05; Ihst2 : Int14u := 8004.0; Rm_Ap : Aint3u50 := (2.0, 2.0, 2.0); Rrmth : Aint15u50 := (14910.0, 14766.0, 14560.0); Rrith : Aint16u50 := (43993.0, 43972.0, 43951.0); Ro : Int18u := 141755.0; Hr : Int15u := 1223.0; Ha : Aint15u11 := (0.0, 2438.0, 4876.0, 7315.0, 9754.0, 12192.0, 14630.0, 17069.0, 19507.0, 21946.0, 25000.0); Tdp : Aint18u4 := (0.0, 53655.0, 100000.0, 200000.0); Ivm2 : Int12s := 1220.0; Ivt2 : Int12s := 270.0; Atari : Int3s := 2.0; Hst2 : Int14u; M_Ap : Int3u; Rmth : Int15u; Rith : Int16u; Vm2, Vt2 : Int12s; Six : constant Int3u := 6.0; Two : constant Int3u := 2.0; Ten : constant Int14u := 10.0; One : constant Int3u := 1.0; Iter, Noiter, N, A, B : Integer; function Square_Root (A : Real) return Real is -- This function determines the square root of the -- given value and returns a result of type realf. X, Z : Real; Xlow, Xhi : Real; begin -- square_root Xlow := abs (A) - abs (A / 10000.0); Xhi := abs (A) + abs (A / 10000.0); X := abs (A); Z := X / Two; while Z ** 2 > Xhi or Z ** 2 < Xlow loop Z := (Z + (X / Z)) * Half; end loop; return Z; end Square_Root; procedure Start (Number_Of_Repetitions : in Integer; Time_Required : out Duration) is Start_Time, End_Time : Duration; Repeats : Integer; procedure S2sp is --------- ---- -- This procedure calculates estimated initial miss distance -- variance. begin -- s2sp S2s := (((Rith / Ro) ** 2) * ((Rmth / Ro) ** 2) * Atari) * S2rno + S2fn + S2sno * (R_Mto / (Rmth)) ** 2; end S2sp; begin -- START Start_Time := Cpu_Time_Clock; Repeats := Number_Of_Repetitions; while Repeats > 0 loop --initialization Noiter := 4; Iter := 1; Hst2 := Ihst2; Ivmi2 := Iivmi2; Ivmj2 := Iivmj2; Ivmk2 := Iivmk2; Ivti2 := Iivti2; Ivtj2 := Iivtj2; Ivtk2 := Iivtk2; Jai := Ijai; Jaj := Ijaj; Jak := Ijak; Japi := Ijapi; Japj := Ijapj; Japk := Ijapk; Kai := Ikai; Kaj := Ikaj; Kak := Ikak; Kapi := Ikapi; Kapj := Ikapj; Kapk := Ikapk; Npua := Inpua; Nyua := Inyua; Vm2 := Ivm2; Vt2 := Ivt2; M_Ap := Rm_Ap (Iter); Rith := Rrith (Iter); Rmth := Rrmth (Iter); Ts := Rts (Iter); --end of initialization -- Calculate predicted miss distance and pitch and yaw Ypdh := Jak * (Vt2 * Ivtk2 - Vm2 * Ivmk2) + Jaj * (Vt2 * Ivtj2 - Vm2 * Ivmj2) + Jai * (Vt2 * Ivti2 - Vm2 * Ivmi2); Yydh := Kak * (Vt2 * Ivtk2 - Vm2 * Ivmk2) + Kaj * (Vt2 * Ivtj2 - Vm2 * Ivmj2) + Kai * (Vt2 * Ivti2 - Vm2 * Ivmi2); N1ps := Jak * (Npua * Japk + Nyua * Kapk) + Jaj * (Npua * Japj + Nyua * Kapj) + Jai * (Npua * Japi + Nyua * Kapi); N1ys := Kak * (Npua * Japk + Nyua * Kapk) + Kaj * (Npua * Japj + Nyua * Kapj) + Kai * (Npua * Japi + Nyua * Kapi); Nnpc := N1ps; Nnyc := N1ys; -- Calculate parameters establishing initial elements -- of the covariance matrix. N := 11; while N > 0 loop exit when Ha (N) <= Hst2 + Hr; N := N - 1; end loop; Rdt := Rd (N) + ((Hst2 + Hr - Ha (N)) / (Ha (N + 1) - Ha (N))) * (Rd (N + 1) - Rd (N)); Realt := Rdt * Half * Vt2 ** 2; N := 3; while N > 0 loop exit when Tdp (N) <= Realt and Realt < Tdp (N + 1); N := N - 1; end loop; Betai := Beta (N) + (Beta (N + 1) - Beta (N)) * (Rdt * Vt2 ** 2 * Half - Tdp (N)) / (Tdp (N + 1) - Tdp (N)); S2sp; --compute s2s -- Set the initial covariance matrix Mp11 := S2s * Rmth ** 2 / Ten; Mp22 := Vm2 ** 2 * S2he; Mp33 := Betai ** 2; Mp21 := D12 * Square_Root (Mp11) * Square_Root (Mp22); Mp31 := D13 * Square_Root (Mp11) * Betai; Mp32 := D23 * Square_Root (Mp22) * Betai; -- Calculate constant terms of covariance matrix prediction -- algorithm. for J in 1 .. 2 loop Q21 (J) := Q21p (J) * Mp33; Q31 (J) := Q31p (J) * Mp33; Q22 (J) := Q22p (J) * Mp33; Q32 (J) := Q32p (J) * Mp33; Q33 (J) := Q33p (J) * Mp33; end loop; <> if Ts = C05 then A := 2; else A := 1; end if; -- Prediction matrix Mp_33 := Phi33 (A) ** 2 * Mp33 + Q33 (A); Mp_22 := Ts * (Ts * Mp33 + Two * Mp32) + Mp22 + Q22 (A); Mp_32 := Phi33 (A) * (Ts * Mp33 + Mp32) + Q32 (A); Mp_31 := Phi33 (A) * (Phi13 (A) * Mp33 + Ts * Mp32 + Mp31) + Q31 (A); Mp_21 := Phi13 (A) * Mp32 + Ts * Mp22 + Mp21 + Ts * (Phi13 (A) * Mp33 + Ts * Mp32 + Mp31) + Q21 (A); Mp_11 := Ts * (Phi13 (A) * Mp32 + Ts * Mp22 + Two * Mp21) + Phi13 (A) * (Phi13 (A) * Mp33 + Ts * Mp32 + Two * Mp31) + Mp11 + Q21 (A); if M_Ap = Six and Ts = C05 then B := Integer (M_Ap) + 2; else B := Integer (M_Ap) + 1; end if; -- Set pitch and yaw commands N1ps := Cc34 (B) * Nnpc + Cc35 (B) * N1ps; N1ys := Cc34 (B) * Nnyc + Cc35 (B) * N1ys; Iter := Iter + 1; if Iter /= Noiter then M_Ap := Rm_Ap (Iter); Rith := Rrith (Iter); Rmth := Rrmth (Iter); Ts := Rts (Iter); S2sp; --compute s2s -- Calculate Kalman gains K1 := Mp_11 / (Mp_11 / Rmth + S2s * Rmth); K2 := Mp_21 / (Mp_11 / Rmth + S2s * Rmth); K3 := Mp_31 / (Mp_11 / Rmth + S2s * Rmth); -- Update smoothed matrix Mp11 := Mp_11 * (One - K1 / Rmth); Mp21 := Mp_21 * (One - K1 / Rmth); Mp31 := Mp_31 * (One - K1 / Rmth); Mp22 := Mp_21 * (-K2 / Rmth) + Mp_22; Mp32 := Mp_21 * (-K3 / Rmth) + Mp_32; Mp33 := Mp_31 * (-K3 / Rmth) + Mp_33; goto Reenter; end if; Repeats := Repeats - 1; end loop; End_Time := Cpu_Time_Clock; Time_Required := End_Time - Start_Time; end Start; begin -- B000003a null; end B000003a;