with Calendar; use Calendar; package body B000001a is -- declare the necessary types and subtypes type Realf is digits 6 range -1.0E+9 .. 1.0E+9; type Real is delta 2.0 ** (-15) range -32767.0 .. 32767.0; type Realx is delta 2.0 ** (-29) range -2.0 .. 2.0; subtype Real15 is Real delta 2.0 ** (-15) range -32767.0 .. 32767.0; subtype Real14 is Real delta 2.0 ** (-15) range -16383.0 .. 16383.0; subtype Real12f is Realf range -4095.0 .. 4095.0; subtype Real11f is Realf range -2047.0 .. 2047.0; subtype Real11 is Real delta 2.0 ** (-15) range -2047.0 .. 2047.0; subtype Real10 is Real delta 2.0 ** (-15) range -1023.0 .. 1023.0; subtype Real9 is Real delta 2.0 ** (-15) range -511.0 .. 511.0; subtype Real5 is Real delta 2.0 ** (-15) range -127.0 .. 127.0; subtype Real8u is Real delta 2.0 ** (-15) range 0.0 .. 255.0; subtype Real6u is Real delta 2.0 ** (-15) range 0.0 .. 63.0; subtype Real4u is Real delta 2.0 ** (-15) range 0.0 .. 15.0; subtype Real3uf is Realf range 0.0 .. 7.0; subtype Real1u is Real delta 2.0 ** (-15) range 0.0 .. 2.0; subtype Fract is Real delta 2.0 ** (-15) range -1.0 .. 1.0; subtype Fractu is Real delta 2.0 ** (-15) range 0.0 .. 1.0; subtype Fractn is Real delta 2.0 ** (-15) range -1.0 .. 0.0; subtype Fractuf is Realf range 0.0 .. 1.0; subtype Fractnf is Realf range -1.0 .. 0.0; subtype Fract5u is Realx delta 2.0 ** (-29) range 0.0 .. 0.0312; subtype Fract17u2 is Realx delta 2.0 ** (-29) range 0.0 .. 0.762E-5; subtype Fract13u2 is Realx delta 2.0 ** (-29) range 0.0 .. 0.122E-3; subtype Fract12u2 is Realx delta 2.0 ** (-15) 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 Afractu2f is array (1 .. 2) of Fractuf; type Afractu2 is array (1 .. 2) of Fractu; type Afractu8 is array (1 .. 8) of Fractu; type Areal3uf is array (1 .. 2) of Real3uf; type Areal1u is array (1 .. 11) of Real1u; type Areal6u is array (1 .. 4) of Real6u; type Integ is range -1E+9 .. 1E+9; subtype Int18u is Integ range 0 .. 262143; subtype Int16u is Integ range 0 .. 65535; subtype Int15u is Integ range 0 .. 32767; subtype Int14uf is Realf range 0.0 .. 16383.0; subtype Int14u is Integ range 0 .. 16383; subtype Int12sf is Realf range -2047.0 .. 2047.0; subtype Int3u is Integ range 0 .. 7; subtype Int3sx 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 : Fractuf := 0.34619; Iivmj2 : Fractnf := -0.04711; Iivmk2 : Fractuf := 0.93698; Iivti2 : Fractuf := 0.34198; Iivtj2 : Fractuf := 0.041294; Iivtk2 : Fractnf := -0.93959; Ijai : Fractnf := -0.67405; Ijaj : Fractuf := 0.70539; Ijak : Fractuf := 0.21923; Ijapi : Fractnf := -0.653465; Ijapj : Fractuf := 0.705007; Ijapk : Fractuf := 0.275583; Ikai : Fractnf := -0.69815; Ikaj : Fractuf := 0.70532; Ikak : Fractuf := 0.12287; Ikapi : Fractnf := -0.664248; Ikapj : Fractnf := -0.70866; Ikapk : Fractuf := 0.237857; Inpua : Real11f := -17.12; Inyua : Real11f := 5.98; Rts : Afractu := (0.1, 0.1, 0.1); Ivmi2, Ivmk2, Ivti2, Ivtj2, Jaj, Jak, Japj, Japk, Kaj, Kak, Kapk : Fractuf; Ivmj2, Ivtk2, Jai, Japi, Kai, Kapi, Kapj : Fractnf; Npua, Nyua : Real11f; Ts : Fractu; K1, K2, K3 : Real12f; 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 : Afractu2f; Rdt : Fractu; Q33 : Areal3uf; Betai : Real6u; 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 : Fract5u := 0.1E-5; S2he : Fract5u := 0.1225E-2; S2rno : Fract5u := 0.9216E-2; S2sno : Fract5u := 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 : Real15; Half : constant Real := 0.5; C05 : constant Real := 0.05; Ihst2 : Int14u := 8004; Rm_Ap : Aint3u50 := (2, 2, 2); Rrmth : Aint15u50 := (14910, 14766, 14560); Rrith : Aint16u50 := (43993, 43972, 43951); Ro : Int18u := 141755; Hr : Int15u := 1223; Ha : Aint15u11 := (0, 2438, 4876, 7315, 9754, 12192, 14630, 17069, 19507, 21946, 25000); Tdp : Aint18u4 := (0, 53655, 100000, 200000); Ivm2 : Real12f := 1220.0; Ivt2 : Real12f := 270.0; Atari : Real4u := 2.0; Hst2 : Int14u; M_Ap : Int3u; Rmth : Int15u; Rith : Int16u; Vm2, Vt2 : Real12f; Six : constant Int3u := 6; Two : constant Int3u := 2; Ten : constant Int14u := 10; One : constant Int3u := 1; Iter, Noiter, N, A, B : Integer; function Square_Root (A : Realf) return Realf is -- This function determines the square root of the -- given value and returns a result of type realf. X, Z : Realf; Xlow, Xhi : Realf; begin Xlow := abs (A) - abs (A / 10000.0); Xhi := abs (A) + abs (A / 10000.0); X := abs (A); Z := X / 2.0; while Z ** 2 > Xhi or Z ** 2 < Xlow loop Z := (Z + (X / Z)) * 0.5; 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 := Fract5u (Fract5u (Fract5u (((Realf (Rith) / Realf (Ro)) ** 2) -- 1 2 345 6 6 6 65 4 * ((Realf (Rmth) / Realf (Ro)) ** 2) -- 45 6 6 6 65 4 * Realf (Atari)) * S2rno) -- 4 43 2 + S2fn + Fract5u (S2sno * Fract5u (Fract5u (R_Mto / Integer (Rmth)) * Fract5u (R_Mto / Integer (Rmth))))); end S2sp; begin -- START Start_Time := Cpu_Time_Clock; -- get initial time 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 distances and pitch and yaw Ypdh := Real5 (Jak * (Vt2 * Ivtk2 - Vm2 * Ivmk2) + Jaj * (Vt2 * Ivtj2 - Vm2 * Ivmj2) + Jai * (Vt2 * Ivti2 - Vm2 * Ivmi2)); Yydh := Real5 (Kak * (Vt2 * Ivtk2 - Vm2 * Ivmk2) + Kaj * (Vt2 * Ivtj2 - Vm2 * Ivmj2) + Kai * (Vt2 * Ivti2 - Vm2 * Ivmi2)); N1ps := Real5 (Jak * (Npua * Japk + Nyua * Kapk) + Jaj * (Npua * Japj + Nyua * Kapj) + Jai * (Npua * Japi + Nyua * Kapi)); N1ys := Real5 (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) + Fract (Fract (Real14 (Hst2 + Hr - Ha (N)) / Real14 (Ha (N + 1) - Ha (N))) * (Rd (N + 1) - Rd (N))); Realt := Real15 (Realf (Rdt) * 0.5 * Vt2 ** 2); N := 3; while N > 0 loop exit when Tdp (N) <= Int15u (Realt) and Int15u (Realt) < Tdp (N + 1); N := N - 1; end loop; Betai := Beta (N) + Real6u ((Beta (N + 1) - Beta (N)) * Real6u ((Realt - Real15 (Tdp (N))) / Integer (Tdp (N + 1) - Tdp (N)))); S2sp; --compute s2s -- set the initial covariance matrix Mp11 := Real10 (Realf (S2s) * Realf (Integer (Rmth ** 2) / 10)); Mp22 := Real11 (Vm2 ** 2 * Realf (S2he)); Mp33 := Real11 (Betai * Betai); Mp21 := Real10 (D12 * Real15 (Square_Root (Realf (Mp11)) * Square_Root (Realf (Mp22)))); Mp31 := Real11 (D13 * Real15 (Square_Root (Realf (Mp11)) * Realf (Betai))); Mp32 := Real11 (D23 * Real15 (Square_Root (Realf (Mp22)) * Realf (Betai))); -- Calculate constant terms of covariance matrix prediction -- algorithm. for J in 1 .. 2 loop Q21 (J) := Fractuf (Fractu (Realf (Q21p (J)) * Realf (Mp33))); Q31 (J) := Fractuf (Fractu (Realf (Q31p (J)) * Realf (Mp33))); Q22 (J) := Fractuf (Fractu (Realf (Q22p (J)) * Realf (Mp33))); Q32 (J) := Fractuf (Fractu (Q32p (J) * Mp33)); Q33 (J) := Real3uf (Real4u (Q33p (J) * Mp33)); end loop; <> if Ts = 0.05 then A := 2; else A := 1; end if; -- prediction matrix Mp_33 := Real11 (Fractu (Phi33 (A) * Phi33 (A)) * Mp33) + Real11 (Q33 (A)); Mp_22 := Real11 (Ts * (Real11 (Ts * Mp33) + Real11 (2 * Mp32))) + Mp22 + Real11 (Q22 (A)); Mp_32 := Real11 (Phi33 (A) * (Real11 (Ts * Mp33) + Mp32)) + Real11 (Q32 (A)); Mp_31 := Real11 (Phi33 (A) * (Real11 (Phi13 (A) * Mp33) + Real11 (Ts * Mp32) + Mp31)) + Real11 (Q31 (A)); Mp_21 := Real10 (Phi13 (A) * Mp32) + Real10 (Ts * Mp22) + Mp21 + Real10 (Ts * (Real10 (Phi13 (A) * Mp33) + Real10 (Ts * Mp32) + Mp31)) + Real10 (Q21 (A)); Mp_11 := Real10 (Ts * (Real10 (Phi13 (A) * Mp32) + Real10 (Ts * Mp22) + Real11 (2 * Mp21))) + Real10 (Phi13 (A) * (Real10 (Phi13 (A) * Mp33) + Real10 (Ts * Mp32) + Real10 (2 * Mp31))) + Mp11 + Real10 (Q21 (A)); if M_Ap = 6 and Ts = 0.05 then B := Integer (M_Ap + 2); else B := Integer (M_Ap + 1); end if; -- set pitch and yaw commands N1ps := Real5 (Cc34 (B) * Nnpc) + Real5 (Cc35 (B) * N1ps); N1ys := Real5 (Cc34 (B) * Nnyc) + Real5 (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 estimated initial miss distance variance -- Calculate Kalman gains K1 := Real12f (Mp_11 / (Fractu (Mp_11 / Integer (Rmth)) + Fractu (Realf (S2s) * Realf (Rmth)))); K2 := Real12f (Mp_21 / (Fractu (Mp_11 / Integer (Rmth)) + Fractu (Realf (S2s) * Realf (Rmth)))); K3 := Real12f (Mp_31 / (Fractu (Mp_11 / Integer (Rmth)) + Fractu (Realf (S2s) * Realf (Rmth)))); -- Update smoothed matrix Mp11 := Real10 (Mp_11 * (1.0 - Fractu (K1 / Realf (Rmth)))); Mp21 := Real10 (Mp_21 * (1.0 - Fractu (K1 / Realf (Rmth)))); Mp31 := Real11 (Mp_31 * (1.0 - Fractu (K1 / Realf (Rmth)))); Mp22 := Real11 (Mp_21 * Fractn (-K2 / Realf (Rmth))) + Mp_22; Mp32 := Real11 (Mp_21 * Fractn (-K3 / Realf (Rmth))) + Mp_32; Mp33 := Real11 (Mp_31 * Fractn (-K3 / Realf (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 -- B000001a null; end B000001a;