with Calendar; use Calendar; package body B000004a is pragma Optimize (Time); -- Declare the necessary types and subtypes type Real is digits 6 range -1.0E+9 .. 1.0E+9; subtype Real15 is Real range -32767.0 .. 32767.0; 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; 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 Int14u is Integ range 0 .. 16383; subtype Int12s is Integ range -2047 .. 2047; subtype Int3u is Integ range 0 .. 7; subtype Int3s is Integ range -3 .. 3; 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 : 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 : 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; 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 : Int12s := 1220; Ivt2 : Int12s := 270; Atari : Int3s := 2; Hst2 : Int14u; M_Ap : Int3u; Rmth : Int15u; Rith : Int16u; Vm2, Vt2 : Int12s; 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 : Real) return Real is -- This function determines the square root of the -- given value and returns a result of type realf. Xlow, Xhi : Real; X, Z : Real; begin -- square_root 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)) * 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 calculate estimated initial miss distance -- variance. begin -- s2sp S2s := ((Real (Rith) / Real (Ro)) ** 2) * ((Real (Rmth) / Real (Ro)) ** 2) * Real (Atari) * S2rno + S2fn + S2sno * (R_Mto / Real (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 distances and pitch and yaw Ypdh := Jak * (Real9 (Vt2) * Ivtk2 - Real11 (Vm2) * Ivmk2) ---- ------ + Jaj * (Real9 (Vt2) * Ivtj2 - Real11 (Vm2) * Ivmj2) ----- ------ + Jai * (Real9 (Vt2) * Ivti2 - Real11 (Vm2) * Ivmi2); ----- ------ Yydh := Kak * (Real9 (Vt2) * Ivtk2 - Real11 (Vm2) * Ivmk2) ----- ------ + Kaj * (Real9 (Vt2) * Ivtj2 - Real11 (Vm2) * Ivmj2) ----- ------ + Kai * (Real9 (Vt2) * Ivti2 - Real11 (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) + (Real15 (Hst2 + Hr - Ha (N)) ------ / Real15 (Ha (N + 1) - Ha (N))) * (Rd (N + 1) - Rd (N)); Realt := Rdt * Half * Real (Vt2) ** 2; ---- N := 3; while N > 0 loop exit when Real (Tdp (N)) <= Realt and Realt < Real (Tdp (N + 1)); ---- ---- N := N - 1; end loop; Betai := Beta (N) + (Beta (N + 1) - Beta (N)) * (Rdt * Real (Vt2) ** 2 * Half - Real (Tdp (N))) ---- ---- / (Real (Tdp (N + 1) - Tdp (N))); ---- S2sp; --compute s2s -- Set the initial covariance matrix Mp11 := S2s * Real (Rmth) ** 2 / Real4u (Ten); ---- ------ Mp22 := Real (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 + Real3u (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 + Real3u (Two) * Mp21) ------ + Phi13 (A) * (Phi13 (A) * Mp33 + Ts * Mp32 + Real3u (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 / Real (Rmth) + S2s * Real (Rmth)); ---- ---- K2 := Mp_21 / (Mp_11 / Real (Rmth) + S2s * Real (Rmth)); ---- ---- K3 := Mp_31 / (Mp_11 / Real (Rmth) + S2s * Real (Rmth)); ---- ---- -- Update matrix Mp11 := Mp_11 * (Real1u (One) - K1 / Real (Rmth)); ---- ---- Mp21 := Mp_21 * (Real1u (One) - K1 / Real (Rmth)); ---- ---- Mp31 := Mp_31 * (Real1u (One) - K1 / Real (Rmth)); ---- ---- Mp22 := Mp_21 * (-K2 / Real (Rmth)) + Mp_22; ---- Mp32 := Mp_21 * (-K3 / Real (Rmth)) + Mp_32; ---- Mp33 := Mp_31 * (-K3 / Real (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 -- B000004a null; end B000004a;