aero-agent-roles 1.0.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/NOTICE +18 -0
- package/README.md +49 -0
- package/bin/aero-agent-roles.js +127 -0
- package/lib/catalog.js +97 -0
- package/lib/install.js +48 -0
- package/lib/mcp.js +199 -0
- package/manifest.json +516 -0
- package/package.json +44 -0
- package/roles/aerodynamics-engineer/ROLE.md +153 -0
- package/roles/aerodynamics-engineer/SOURCES.md +12 -0
- package/roles/aerodynamics-engineer/cli.py +143 -0
- package/roles/aerodynamics-engineer/core/aerodynamics_core.py +1218 -0
- package/roles/aerodynamics-engineer/templates/aero-design-report-template.md +72 -0
- package/roles/aerodynamics-engineer/tests/test_aerodynamics_core.py +227 -0
- package/roles/aerodynamics-engineer/tests/test_role_aerodynamics.py +94 -0
- package/roles/aircraft-design-engineer/ROLE.md +138 -0
- package/roles/aircraft-design-engineer/SOURCES.md +12 -0
- package/roles/aircraft-design-engineer/cli.py +152 -0
- package/roles/aircraft-design-engineer/core/aircraft_design_core.py +1278 -0
- package/roles/aircraft-design-engineer/templates/concept-design-template.md +99 -0
- package/roles/aircraft-design-engineer/tests/test_aircraft_design_core.py +269 -0
- package/roles/aircraft-design-engineer/tests/test_role_aircraft_design_engineer.py +106 -0
- package/roles/airworthiness-compliance-engineer/ROLE.md +122 -0
- package/roles/airworthiness-compliance-engineer/SOURCES.md +16 -0
- package/roles/airworthiness-compliance-engineer/cli.py +146 -0
- package/roles/airworthiness-compliance-engineer/core/airworthiness_core.py +894 -0
- package/roles/airworthiness-compliance-engineer/templates/compliance-matrix-template.md +46 -0
- package/roles/airworthiness-compliance-engineer/tests/test_airworthiness_core.py +224 -0
- package/roles/airworthiness-compliance-engineer/tests/test_role_airworthiness.py +85 -0
- package/roles/as9100-quality-auditor/ROLE.md +128 -0
- package/roles/as9100-quality-auditor/SOURCES.md +16 -0
- package/roles/as9100-quality-auditor/cli.py +136 -0
- package/roles/as9100-quality-auditor/core/as9100_core.py +857 -0
- package/roles/as9100-quality-auditor/templates/findings-report-template.md +119 -0
- package/roles/as9100-quality-auditor/tests/test_as9100_cli_profile.py +105 -0
- package/roles/as9100-quality-auditor/tests/test_as9100_core.py +204 -0
- package/roles/as9100-quality-auditor/tests/test_role_as9100.py +90 -0
- package/roles/data-bus-avionics-engineer/ROLE.md +146 -0
- package/roles/data-bus-avionics-engineer/SOURCES.md +23 -0
- package/roles/data-bus-avionics-engineer/cli.py +311 -0
- package/roles/data-bus-avionics-engineer/core/data_bus_avionics_core.py +1121 -0
- package/roles/data-bus-avionics-engineer/templates/bus-assessment-template.md +134 -0
- package/roles/data-bus-avionics-engineer/tests/test_bundle_protocol.py +137 -0
- package/roles/data-bus-avionics-engineer/tests/test_data_bus_avionics_core.py +261 -0
- package/roles/data-bus-avionics-engineer/tests/test_role_data_bus_avionics_engineer.py +121 -0
- package/roles/do160-environmental-engineer/ROLE.md +136 -0
- package/roles/do160-environmental-engineer/SOURCES.md +30 -0
- package/roles/do160-environmental-engineer/cli.py +147 -0
- package/roles/do160-environmental-engineer/core/do160_environmental_core.py +1246 -0
- package/roles/do160-environmental-engineer/templates/environmental-qualification-plan-template.md +114 -0
- package/roles/do160-environmental-engineer/tests/test_do160_environmental_core.py +312 -0
- package/roles/do160-environmental-engineer/tests/test_role_do160_environmental_engineer.py +281 -0
- package/roles/do178c-cert-engineer/ROLE.md +120 -0
- package/roles/do178c-cert-engineer/SOURCES.md +16 -0
- package/roles/do178c-cert-engineer/cli.py +129 -0
- package/roles/do178c-cert-engineer/core/do178c_core.py +411 -0
- package/roles/do178c-cert-engineer/templates/psac-template.md +74 -0
- package/roles/do178c-cert-engineer/tests/test_do178c_cert_engineer_core.py +91 -0
- package/roles/do178c-cert-engineer/tests/test_role_do178c.py +109 -0
- package/roles/do254-hardware-engineer/ROLE.md +127 -0
- package/roles/do254-hardware-engineer/SOURCES.md +19 -0
- package/roles/do254-hardware-engineer/cli.py +308 -0
- package/roles/do254-hardware-engineer/core/do254_hardware_core.py +761 -0
- package/roles/do254-hardware-engineer/templates/phac-template.md +87 -0
- package/roles/do254-hardware-engineer/tests/test_do254_bundle_profile.py +152 -0
- package/roles/do254-hardware-engineer/tests/test_do254_hardware_core.py +200 -0
- package/roles/do254-hardware-engineer/tests/test_role_do254_hardware.py +108 -0
- package/roles/engineering-analysis-engineer/ROLE.md +154 -0
- package/roles/engineering-analysis-engineer/SOURCES.md +15 -0
- package/roles/engineering-analysis-engineer/cli.py +151 -0
- package/roles/engineering-analysis-engineer/core/engineering_analysis_engineer_core.py +866 -0
- package/roles/engineering-analysis-engineer/templates/analysis-memo-template.md +73 -0
- package/roles/engineering-analysis-engineer/tests/test_engineering_analysis_engineer_core.py +367 -0
- package/roles/engineering-analysis-engineer/tests/test_role_engineering_analysis_engineer.py +113 -0
- package/roles/flight-management-engineer/ROLE.md +160 -0
- package/roles/flight-management-engineer/SOURCES.md +30 -0
- package/roles/flight-management-engineer/cli.py +258 -0
- package/roles/flight-management-engineer/core/flight_management_core.py +1473 -0
- package/roles/flight-management-engineer/templates/route-assessment-template.md +94 -0
- package/roles/flight-management-engineer/tests/test_flight_management_core.py +437 -0
- package/roles/flight-management-engineer/tests/test_role_flight_management_engineer.py +123 -0
- package/roles/flight-mechanics-engineer/ROLE.md +162 -0
- package/roles/flight-mechanics-engineer/SOURCES.md +51 -0
- package/roles/flight-mechanics-engineer/cli.py +149 -0
- package/roles/flight-mechanics-engineer/core/flight_mechanics_core.py +1507 -0
- package/roles/flight-mechanics-engineer/templates/perf-sc-report-template.md +82 -0
- package/roles/flight-mechanics-engineer/tests/test_flight_mechanics_core.py +277 -0
- package/roles/flight-mechanics-engineer/tests/test_role_flight_mechanics_engineer.py +141 -0
- package/roles/flight-test-engineer/ROLE.md +137 -0
- package/roles/flight-test-engineer/SOURCES.md +24 -0
- package/roles/flight-test-engineer/cli.py +135 -0
- package/roles/flight-test-engineer/core/flight_test_core.py +931 -0
- package/roles/flight-test-engineer/templates/flight-test-template.md +143 -0
- package/roles/flight-test-engineer/tests/test_flight_test_engineer_core.py +227 -0
- package/roles/flight-test-engineer/tests/test_role_flight_test_engineer.py +150 -0
- package/roles/flight-test-performance-engineer/ROLE.md +165 -0
- package/roles/flight-test-performance-engineer/SOURCES.md +20 -0
- package/roles/flight-test-performance-engineer/cli.py +319 -0
- package/roles/flight-test-performance-engineer/core/flight_test_performance_core.py +1398 -0
- package/roles/flight-test-performance-engineer/templates/performance-report-template.md +105 -0
- package/roles/flight-test-performance-engineer/tests/test_flight_test_performance_engineer_core.py +308 -0
- package/roles/flight-test-performance-engineer/tests/test_role_flight_test_performance_engineer.py +165 -0
- package/roles/gnc-engineer/ROLE.md +141 -0
- package/roles/gnc-engineer/SOURCES.md +35 -0
- package/roles/gnc-engineer/cli.py +141 -0
- package/roles/gnc-engineer/core/gnc_core.py +1074 -0
- package/roles/gnc-engineer/templates/gnc-report-template.md +105 -0
- package/roles/gnc-engineer/tests/test_gnc_engineer_core.py +238 -0
- package/roles/gnc-engineer/tests/test_role_gnc_engineer.py +142 -0
- package/roles/guidance-engineer/ROLE.md +156 -0
- package/roles/guidance-engineer/SOURCES.md +43 -0
- package/roles/guidance-engineer/cli.py +280 -0
- package/roles/guidance-engineer/core/guidance_core.py +820 -0
- package/roles/guidance-engineer/templates/guidance-report-template.md +70 -0
- package/roles/guidance-engineer/tests/test_guidance_engineer_core.py +269 -0
- package/roles/guidance-engineer/tests/test_role_guidance_engineer.py +146 -0
- package/roles/high-speed-aerodynamics-engineer/ROLE.md +161 -0
- package/roles/high-speed-aerodynamics-engineer/SOURCES.md +18 -0
- package/roles/high-speed-aerodynamics-engineer/cli.py +282 -0
- package/roles/high-speed-aerodynamics-engineer/core/high_speed_aero_core.py +1566 -0
- package/roles/high-speed-aerodynamics-engineer/templates/high-speed-memo-template.md +75 -0
- package/roles/high-speed-aerodynamics-engineer/tests/test_bundle_protocol.py +103 -0
- package/roles/high-speed-aerodynamics-engineer/tests/test_high_speed_aero_core.py +280 -0
- package/roles/high-speed-aerodynamics-engineer/tests/test_role_high_speed_aerodynamics.py +125 -0
- package/roles/ndt-engineer/ROLE.md +157 -0
- package/roles/ndt-engineer/SOURCES.md +18 -0
- package/roles/ndt-engineer/cli.py +316 -0
- package/roles/ndt-engineer/core/ndt_core.py +1590 -0
- package/roles/ndt-engineer/templates/ndt-plan-template.md +109 -0
- package/roles/ndt-engineer/tests/test_ndt_cli_bundle.py +183 -0
- package/roles/ndt-engineer/tests/test_ndt_core.py +437 -0
- package/roles/ndt-engineer/tests/test_role_ndt_engineer.py +148 -0
- package/roles/numerical-analysis-engineer/ROLE.md +158 -0
- package/roles/numerical-analysis-engineer/SOURCES.md +19 -0
- package/roles/numerical-analysis-engineer/cli.py +414 -0
- package/roles/numerical-analysis-engineer/core/numerical_analysis_core.py +1220 -0
- package/roles/numerical-analysis-engineer/templates/verification-memo-template.md +120 -0
- package/roles/numerical-analysis-engineer/tests/test_bundle_protocol.py +157 -0
- package/roles/numerical-analysis-engineer/tests/test_numerical_analysis_core.py +337 -0
- package/roles/numerical-analysis-engineer/tests/test_role_numerical_analysis_engineer.py +117 -0
- package/roles/propulsion-engineer/ROLE.md +163 -0
- package/roles/propulsion-engineer/SOURCES.md +15 -0
- package/roles/propulsion-engineer/cli.py +168 -0
- package/roles/propulsion-engineer/core/propulsion_core.py +1291 -0
- package/roles/propulsion-engineer/templates/propulsion-report-template.md +92 -0
- package/roles/propulsion-engineer/tests/test_propulsion_engineer_core.py +294 -0
- package/roles/propulsion-engineer/tests/test_role_propulsion_engineer.py +106 -0
- package/roles/space-systems-engineer/ROLE.md +161 -0
- package/roles/space-systems-engineer/SOURCES.md +49 -0
- package/roles/space-systems-engineer/cli.py +173 -0
- package/roles/space-systems-engineer/core/space_systems_core.py +980 -0
- package/roles/space-systems-engineer/templates/space-systems-template.md +88 -0
- package/roles/space-systems-engineer/tests/test_role_space_systems_engineer.py +117 -0
- package/roles/space-systems-engineer/tests/test_space_systems_engineer_core.py +247 -0
- package/roles/state-estimation-engineer/ROLE.md +154 -0
- package/roles/state-estimation-engineer/SOURCES.md +27 -0
- package/roles/state-estimation-engineer/cli.py +326 -0
- package/roles/state-estimation-engineer/core/state_estimation_core.py +958 -0
- package/roles/state-estimation-engineer/templates/nav-state-estimator-design-template.md +76 -0
- package/roles/state-estimation-engineer/tests/test_bundle_protocol.py +110 -0
- package/roles/state-estimation-engineer/tests/test_role_state_estimation_engineer.py +127 -0
- package/roles/state-estimation-engineer/tests/test_state_estimation_engineer_core.py +231 -0
- package/roles/structures-loads-engineer/ROLE.md +171 -0
- package/roles/structures-loads-engineer/SOURCES.md +44 -0
- package/roles/structures-loads-engineer/cli.py +219 -0
- package/roles/structures-loads-engineer/core/structures_loads_core.py +1112 -0
- package/roles/structures-loads-engineer/templates/loads-strength-report-template.md +96 -0
- package/roles/structures-loads-engineer/tests/test_bundle_protocol.py +101 -0
- package/roles/structures-loads-engineer/tests/test_role_structures_loads_engineer.py +147 -0
- package/roles/structures-loads-engineer/tests/test_structures_loads_core.py +261 -0
|
@@ -0,0 +1,1074 @@
|
|
|
1
|
+
#!/usr/bin/env python3
|
|
2
|
+
"""gnc_core.py - GNC Engineer executable core.
|
|
3
|
+
|
|
4
|
+
This is the role's ENGINE: given a vehicle's GNC project facts (plant
|
|
5
|
+
model, sensor noise assumptions, design targets) it computes controller
|
|
6
|
+
gains (PID pole placement, LQR Riccati solution, observer gains),
|
|
7
|
+
stability margins (phase/gain margin from the loop gain), navigation
|
|
8
|
+
filter steady state (Kalman covariance and gain), guidance commands
|
|
9
|
+
(proportional navigation lateral acceleration), digital design checks
|
|
10
|
+
(sample-rate rule, discrete PID coefficients), and a seeded Monte Carlo
|
|
11
|
+
robustness pass. It BUILDS the GNC design report content and
|
|
12
|
+
gate-checks deliverables. Standalone: stdlib only, no AeroSkills
|
|
13
|
+
checkout required.
|
|
14
|
+
|
|
15
|
+
Domain rules encoded here are the standard textbook methods carried by
|
|
16
|
+
the bound Aero Agent Skills leaves (which encode aircraft flight
|
|
17
|
+
control / estimation / guidance practice):
|
|
18
|
+
|
|
19
|
+
- PID design: closed-loop pole placement matching the plant to
|
|
20
|
+
s^2 + 2*zeta*wn*s + wn^2 (and an extra real pole at -p3), from
|
|
21
|
+
gnc-autonomy/control/pid-control-design. Natural frequency wn is
|
|
22
|
+
sized from the required bandwidth via the exact second-order
|
|
23
|
+
bandwidth relation wn_bw = wn*sqrt(1 - 2z^2 + sqrt(2 - 4z^2 + 4z^4)).
|
|
24
|
+
- Margins: type-1 open loop L(s) = K/(s(s + a)) closed-form phase
|
|
25
|
+
margin (finite) and gain margin (infinite); general loop margins
|
|
26
|
+
evaluated on the imaginary axis with root-sum unwrapped phase, per
|
|
27
|
+
gnc-autonomy/control/frequency-response-design.
|
|
28
|
+
- LQR: K = R^-1 B' P from the algebraic Riccati equation closed form
|
|
29
|
+
for the canonical two-state plant, per gnc-autonomy/optimal-control/
|
|
30
|
+
lqr-design.
|
|
31
|
+
- Kalman: steady-state predicted covariance (scalar Riccati root) and
|
|
32
|
+
gain K = h P/(h^2 P + r), per gnc-autonomy/navigation/
|
|
33
|
+
kalman-filter-design.
|
|
34
|
+
- Observer: full-order Ackermann gain for the two-state plant, per
|
|
35
|
+
gnc-autonomy/control/observer-design.
|
|
36
|
+
- Guidance: proportional navigation lateral acceleration
|
|
37
|
+
a_c = N*Vc*lam_dot with Vc and lam_dot from the relative state, per
|
|
38
|
+
gnc-autonomy/guidance/pursuit-guidance.
|
|
39
|
+
- Digital: 10-20 samples per closed-loop cycle rule and discrete PID
|
|
40
|
+
velocity-form coefficients, per gnc-autonomy/control/
|
|
41
|
+
digital-control-design.
|
|
42
|
+
|
|
43
|
+
MIL-STD-1797A (TIER-1, reference-only=false per standards-map) supplies
|
|
44
|
+
handling-qualities context only; no standard text is reproduced.
|
|
45
|
+
"""
|
|
46
|
+
from __future__ import annotations
|
|
47
|
+
|
|
48
|
+
import cmath
|
|
49
|
+
import math
|
|
50
|
+
import random
|
|
51
|
+
from dataclasses import dataclass, field
|
|
52
|
+
from datetime import date
|
|
53
|
+
|
|
54
|
+
def _today() -> str:
|
|
55
|
+
import os
|
|
56
|
+
from datetime import date
|
|
57
|
+
return os.environ.get("ROLE_GEN_DATE", date.today().isoformat())
|
|
58
|
+
|
|
59
|
+
# ---------------------------------------------------------------------------
|
|
60
|
+
# Domain rules (textbook formulas carried by the bound leaves)
|
|
61
|
+
# ---------------------------------------------------------------------------
|
|
62
|
+
|
|
63
|
+
_SQRT2 = math.sqrt(2.0)
|
|
64
|
+
|
|
65
|
+
|
|
66
|
+
def second_order_bandwidth_factor(zeta: float) -> float:
|
|
67
|
+
"""Exact closed-loop bandwidth factor f(z) for s^2 + 2zw s + w^2:
|
|
68
|
+
|
|
69
|
+
f = sqrt(1 - 2z^2 + sqrt(2 - 4z^2 + 4z^4)), w_bw = f * wn.
|
|
70
|
+
f(0.7071) == 1 exactly (bandwidth equals natural frequency at the
|
|
71
|
+
classic 0.707 damping). Requires 0 < zeta <= 1.
|
|
72
|
+
"""
|
|
73
|
+
if not (0.0 < zeta <= 1.0):
|
|
74
|
+
raise ValueError("zeta must lie in (0, 1], got %r" % (zeta,))
|
|
75
|
+
return math.sqrt(1.0 - 2.0 * zeta * zeta
|
|
76
|
+
+ math.sqrt(2.0 - 4.0 * zeta * zeta + 4.0 * zeta ** 4))
|
|
77
|
+
|
|
78
|
+
|
|
79
|
+
def natural_frequency_from_bandwidth(w_bw: float, zeta: float) -> float:
|
|
80
|
+
"""Size the natural frequency wn so the closed loop reaches the
|
|
81
|
+
required bandwidth w_bw at damping zeta: wn = w_bw / f(zeta)."""
|
|
82
|
+
if not (isinstance(w_bw, (int, float)) and math.isfinite(w_bw)
|
|
83
|
+
and w_bw > 0.0):
|
|
84
|
+
raise ValueError("w_bw must be a positive finite number, got %r"
|
|
85
|
+
% (w_bw,))
|
|
86
|
+
return w_bw / second_order_bandwidth_factor(zeta)
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
def closed_loop_bandwidth(wn: float, zeta: float) -> float:
|
|
90
|
+
"""Bandwidth achieved by a second-order loop with wn, zeta."""
|
|
91
|
+
if not (isinstance(wn, (int, float)) and math.isfinite(wn) and wn > 0.0):
|
|
92
|
+
raise ValueError("wn must be a positive finite number, got %r" % (wn,))
|
|
93
|
+
return wn * second_order_bandwidth_factor(zeta)
|
|
94
|
+
|
|
95
|
+
|
|
96
|
+
def pid_gains_first_order(a: float, b: float, wn: float, zeta: float):
|
|
97
|
+
"""PI gains (kp, ki) for G(s) = b/(s + a) matched to
|
|
98
|
+
s^2 + 2*zeta*wn*s + wn^2: kp = (2*zeta*wn - a)/b, ki = wn^2/b."""
|
|
99
|
+
if b == 0:
|
|
100
|
+
raise ValueError("b must be nonzero, got %r" % (b,))
|
|
101
|
+
if not (0.0 < zeta <= 1.0):
|
|
102
|
+
raise ValueError("zeta must lie in (0, 1], got %r" % (zeta,))
|
|
103
|
+
if not (isinstance(wn, (int, float)) and wn > 0.0):
|
|
104
|
+
raise ValueError("wn must be > 0, got %r" % (wn,))
|
|
105
|
+
kp = (2.0 * zeta * wn - a) / b
|
|
106
|
+
ki = wn * wn / b
|
|
107
|
+
return kp, ki
|
|
108
|
+
|
|
109
|
+
|
|
110
|
+
def pid_gains_second_order(a1: float, a0: float, b: float, wn: float,
|
|
111
|
+
zeta: float, p3: float):
|
|
112
|
+
"""PID gains (kp, ki, kd) for G(s) = b/(s^2 + a1 s + a0) matched to
|
|
113
|
+
(s^2 + 2*zeta*wn*s + wn^2)(s + p3):
|
|
114
|
+
kd = (2*zeta*wn + p3 - a1)/b
|
|
115
|
+
kp = (wn^2 + 2*zeta*wn*p3 - a0)/b
|
|
116
|
+
ki = wn^2*p3/b
|
|
117
|
+
"""
|
|
118
|
+
if b == 0:
|
|
119
|
+
raise ValueError("b must be nonzero, got %r" % (b,))
|
|
120
|
+
if not (0.0 < zeta <= 1.0):
|
|
121
|
+
raise ValueError("zeta must lie in (0, 1], got %r" % (zeta,))
|
|
122
|
+
if not (isinstance(wn, (int, float)) and wn > 0.0):
|
|
123
|
+
raise ValueError("wn must be > 0, got %r" % (wn,))
|
|
124
|
+
if not (isinstance(p3, (int, float)) and p3 > 0.0):
|
|
125
|
+
raise ValueError("p3 must be > 0, got %r" % (p3,))
|
|
126
|
+
kd = (2.0 * zeta * wn + p3 - a1) / b
|
|
127
|
+
kp = (wn * wn + 2.0 * zeta * wn * p3 - a0) / b
|
|
128
|
+
ki = wn * wn * p3 / b
|
|
129
|
+
return kp, ki, kd
|
|
130
|
+
|
|
131
|
+
|
|
132
|
+
def type1_margins(a: float, K: float) -> dict:
|
|
133
|
+
"""Gain/phase margin of the type-1 open loop L(s) = K/(s(s + a)).
|
|
134
|
+
|
|
135
|
+
Crossover wc solves K^2 = wc^2(wc^2 + a^2); phase margin is
|
|
136
|
+
90 - atan(wc/a) deg. Phase reaches -180 deg only at w -> inf, so
|
|
137
|
+
the gain margin is infinite (reported as inf)."""
|
|
138
|
+
if not (isinstance(a, (int, float)) and a > 0.0):
|
|
139
|
+
raise ValueError("a must be > 0, got %r" % (a,))
|
|
140
|
+
if not (isinstance(K, (int, float)) and K > 0.0):
|
|
141
|
+
raise ValueError("K must be > 0, got %r" % (K,))
|
|
142
|
+
wc2 = (-a * a + math.sqrt(a ** 4 + 4.0 * K * K)) / 2.0
|
|
143
|
+
wc = math.sqrt(wc2)
|
|
144
|
+
pm = 90.0 - math.degrees(math.atan(wc / a))
|
|
145
|
+
return {"crossover_rad_s": wc, "phase_margin_deg": pm,
|
|
146
|
+
"gain_margin": float("inf"), "gain_margin_db": float("inf")}
|
|
147
|
+
|
|
148
|
+
|
|
149
|
+
def _poly_roots(coeffs):
|
|
150
|
+
"""Roots of a descending-power polynomial with stdlib (deg <= 3)."""
|
|
151
|
+
# strip leading zeros
|
|
152
|
+
c = [float(v) for v in coeffs]
|
|
153
|
+
while c and c[0] == 0.0:
|
|
154
|
+
c.pop(0)
|
|
155
|
+
n = len(c) - 1
|
|
156
|
+
if n <= 0:
|
|
157
|
+
return []
|
|
158
|
+
if n == 1:
|
|
159
|
+
return [complex(-c[1] / c[0], 0.0)]
|
|
160
|
+
if n == 2:
|
|
161
|
+
a, bb, cc = c[0], c[1], c[2]
|
|
162
|
+
disc = cmath.sqrt(bb * bb - 4.0 * a * cc)
|
|
163
|
+
return [(-bb + disc) / (2.0 * a), (-bb - disc) / (2.0 * a)]
|
|
164
|
+
if n == 3:
|
|
165
|
+
# one real root by the trigonometric/Vieta closed form, then
|
|
166
|
+
# synthetic-division deflation to a quadratic
|
|
167
|
+
a, bb, cc, d = c[0], c[1], c[2], c[3]
|
|
168
|
+
p = (3.0 * a * cc - bb * bb) / (3.0 * a * a)
|
|
169
|
+
q = (2.0 * bb ** 3 - 9.0 * a * bb * cc + 27.0 * a * a * d) \
|
|
170
|
+
/ (27.0 * a ** 3)
|
|
171
|
+
disc = (q / 2.0) ** 2 + (p / 3.0) ** 3
|
|
172
|
+
if p == 0.0: # triply-degenerate case
|
|
173
|
+
t1 = math.copysign(abs(q) ** (1.0 / 3.0), -q) if q != 0.0 else 0.0
|
|
174
|
+
elif disc >= 0.0:
|
|
175
|
+
u = math.copysign(abs(-q / 2.0 + math.sqrt(disc)) ** (1.0 / 3.0),
|
|
176
|
+
-q / 2.0 + math.sqrt(disc))
|
|
177
|
+
v = math.copysign(abs(-q / 2.0 - math.sqrt(disc)) ** (1.0 / 3.0),
|
|
178
|
+
-q / 2.0 - math.sqrt(disc))
|
|
179
|
+
t1 = u + v
|
|
180
|
+
else:
|
|
181
|
+
r = math.sqrt((-p / 3.0) ** 3)
|
|
182
|
+
theta = math.acos(min(1.0, max(-1.0, -q / (2.0 * r))))
|
|
183
|
+
t1 = 2.0 * math.sqrt(-p / 3.0) * math.cos(theta / 3.0)
|
|
184
|
+
r1 = t1 - bb / (3.0 * a)
|
|
185
|
+
# synthetic division of c by (s - r1): quotient q1 s^2 + q2 s + q3
|
|
186
|
+
q1 = a
|
|
187
|
+
q2 = bb + q1 * r1
|
|
188
|
+
q3 = cc + q2 * r1
|
|
189
|
+
pair = _poly_roots([q1, q2, q3])
|
|
190
|
+
return [complex(r1, 0.0)] + pair
|
|
191
|
+
raise ValueError("root solving supports degree <= 3, got degree %d" % n)
|
|
192
|
+
|
|
193
|
+
|
|
194
|
+
def _phase_of_loop(num, den, w):
|
|
195
|
+
"""Unwrapped phase in degrees of L(jw) = num(jw)/den(jw) at w > 0.
|
|
196
|
+
|
|
197
|
+
Computed as a root sum: arg L = sum over zeros of arg(jw - z)
|
|
198
|
+
minus sum over poles of arg(jw - p), exact and continuous for w > 0
|
|
199
|
+
when no root lies on the imaginary axis (a pole at the origin
|
|
200
|
+
contributes a constant +90 deg)."""
|
|
201
|
+
zr = _poly_roots(num)
|
|
202
|
+
pr = _poly_roots(den)
|
|
203
|
+
total = 0.0
|
|
204
|
+
for z in zr:
|
|
205
|
+
total += math.degrees(math.atan2(w - z.imag, -z.real))
|
|
206
|
+
for p in pr:
|
|
207
|
+
total -= math.degrees(math.atan2(w - p.imag, -p.real))
|
|
208
|
+
return total
|
|
209
|
+
|
|
210
|
+
|
|
211
|
+
def _mag_db_of_loop(num, den, w):
|
|
212
|
+
"""20*log10 |L(jw)| with exact complex evaluation."""
|
|
213
|
+
s = complex(0.0, w)
|
|
214
|
+
accn = 0.0 + 0.0j
|
|
215
|
+
for c in num:
|
|
216
|
+
accn = accn * s + c
|
|
217
|
+
accd = 0.0 + 0.0j
|
|
218
|
+
for c in den:
|
|
219
|
+
accd = accd * s + c
|
|
220
|
+
return 20.0 * math.log10(abs(accn / accd))
|
|
221
|
+
|
|
222
|
+
|
|
223
|
+
def loop_margins(num: list, den: list) -> dict:
|
|
224
|
+
"""Gain/phase margins of an open loop L(s) = num(s)/den(s).
|
|
225
|
+
|
|
226
|
+
num/den are descending-power coefficient lists. Returns gain
|
|
227
|
+
crossover w_gc, phase crossover w_pc, gain margin (linear and dB,
|
|
228
|
+
inf when the phase never reaches -180 deg), phase margin (deg, at
|
|
229
|
+
w_gc), and the classic verdict booleans (GM > 0 dB, PM > 0 deg).
|
|
230
|
+
Bisection on the log-frequency axis; deterministic."""
|
|
231
|
+
for poly, name in ((num, "num"), (den, "den")):
|
|
232
|
+
if not isinstance(poly, (list, tuple)) or not poly:
|
|
233
|
+
raise ValueError("%s must be a non-empty coefficient list" % name)
|
|
234
|
+
w_lo, w_hi = 1e-3, 1e5
|
|
235
|
+
# gain crossover: |L| = 1 falls monotonically near crossover for the
|
|
236
|
+
# type-1/type-0 loops used here; bracket then bisect
|
|
237
|
+
w_gc = float("inf")
|
|
238
|
+
lo, hi = w_lo, w_hi
|
|
239
|
+
flo = _mag_db_of_loop(num, den, lo)
|
|
240
|
+
fhi = _mag_db_of_loop(num, den, hi)
|
|
241
|
+
if flo > 0.0 and fhi < 0.0:
|
|
242
|
+
for _ in range(100):
|
|
243
|
+
mid = math.sqrt(lo * hi)
|
|
244
|
+
fm = _mag_db_of_loop(num, den, mid)
|
|
245
|
+
if fm > 0.0:
|
|
246
|
+
lo = mid
|
|
247
|
+
else:
|
|
248
|
+
hi = mid
|
|
249
|
+
w_gc = math.sqrt(lo * hi)
|
|
250
|
+
# phase crossover: unwrapped phase == -180 deg
|
|
251
|
+
w_pc = float("inf")
|
|
252
|
+
plo = _phase_of_loop(num, den, w_lo)
|
|
253
|
+
phi = _phase_of_loop(num, den, w_hi)
|
|
254
|
+
if plo > -180.0 and phi < -180.0:
|
|
255
|
+
lo, hi = w_lo, w_hi
|
|
256
|
+
for _ in range(100):
|
|
257
|
+
mid = math.sqrt(lo * hi)
|
|
258
|
+
pm = _phase_of_loop(num, den, mid)
|
|
259
|
+
if pm > -180.0:
|
|
260
|
+
lo = mid
|
|
261
|
+
else:
|
|
262
|
+
hi = mid
|
|
263
|
+
w_pc = math.sqrt(lo * hi)
|
|
264
|
+
if math.isinf(w_pc):
|
|
265
|
+
gm = float("inf")
|
|
266
|
+
else:
|
|
267
|
+
gm = 10.0 ** (-_mag_db_of_loop(num, den, w_pc) / 20.0)
|
|
268
|
+
gm_db = float("inf") if math.isinf(gm) else 20.0 * math.log10(gm)
|
|
269
|
+
pm = float("inf")
|
|
270
|
+
if not math.isinf(w_gc):
|
|
271
|
+
pm = 180.0 + _phase_of_loop(num, den, w_gc)
|
|
272
|
+
return {
|
|
273
|
+
"gain_crossover_rad_s": w_gc,
|
|
274
|
+
"phase_crossover_rad_s": w_pc,
|
|
275
|
+
"gain_margin": gm,
|
|
276
|
+
"gain_margin_db": gm_db,
|
|
277
|
+
"phase_margin_deg": pm,
|
|
278
|
+
"gain_margin_ok": gm_db > 0.0 if not math.isinf(gm_db) else True,
|
|
279
|
+
"phase_margin_ok": pm > 0.0 if not math.isinf(pm) else True,
|
|
280
|
+
}
|
|
281
|
+
|
|
282
|
+
|
|
283
|
+
def riccati_solution(A, B, Q, R) -> dict:
|
|
284
|
+
"""LQR: algebraic Riccati solution P and gain K = R^-1 B' P for the
|
|
285
|
+
canonical plant A = [[0, 1], [0, -a]] (a >= 0), B = [0, 1],
|
|
286
|
+
Q = diag(q1, q2), R > 0. Closed form (common control theory)."""
|
|
287
|
+
_TOL = 1e-9
|
|
288
|
+
try:
|
|
289
|
+
a = -float(A[1][1])
|
|
290
|
+
if abs(A[0][0]) > _TOL or abs(A[0][1] - 1.0) > _TOL \
|
|
291
|
+
or abs(A[1][0]) > _TOL or abs(B[0]) > _TOL \
|
|
292
|
+
or abs(B[1] - 1.0) > _TOL or a < -_TOL:
|
|
293
|
+
raise ValueError("A/B must be canonical [[0,1],[0,-a]], [0,1]")
|
|
294
|
+
q1, q2 = max(float(Q[0][0]), 0.0), max(float(Q[1][1]), 0.0)
|
|
295
|
+
r = float(R)
|
|
296
|
+
except (TypeError, ValueError, IndexError) as exc:
|
|
297
|
+
raise ValueError("invalid canonical LQR data: %s" % exc)
|
|
298
|
+
if r <= 0.0:
|
|
299
|
+
raise ValueError("R must be > 0, got %r" % (R,))
|
|
300
|
+
a = max(a, 0.0)
|
|
301
|
+
p2 = math.sqrt(r * q1)
|
|
302
|
+
p3 = r * (-a + math.sqrt(a * a + (2.0 * p2 + q2) / r))
|
|
303
|
+
p1 = a * p2 + p2 * p3 / r
|
|
304
|
+
P = [[p1, p2], [p2, p3]]
|
|
305
|
+
K = [p2 / r, p3 / r]
|
|
306
|
+
# closed loop A - B K = [[0, 1], [-k1, -a - k2]]: stable iff
|
|
307
|
+
# trace < 0 and det > 0
|
|
308
|
+
trace = -a - K[1]
|
|
309
|
+
det = K[0]
|
|
310
|
+
disc = trace * trace - 4.0 * det
|
|
311
|
+
if disc >= 0.0:
|
|
312
|
+
s = math.sqrt(disc)
|
|
313
|
+
poles = [((trace + s) / 2.0, 0.0), ((trace - s) / 2.0, 0.0)]
|
|
314
|
+
else:
|
|
315
|
+
s = math.sqrt(-disc)
|
|
316
|
+
poles = [(trace / 2.0, s / 2.0), (trace / 2.0, -s / 2.0)]
|
|
317
|
+
return {"P": P, "K": K, "stable": trace < 0.0 and det > 0.0,
|
|
318
|
+
"poles": poles, "damping_a": a}
|
|
319
|
+
|
|
320
|
+
|
|
321
|
+
def kalman_steady_state(f: float, h: float, q: float, r: float) -> dict:
|
|
322
|
+
"""Steady-state scalar Kalman quantities (f, h, q, r constant model).
|
|
323
|
+
|
|
324
|
+
P_inf is the positive root of the scalar Riccati equation
|
|
325
|
+
P = f^2 P - f^2 P h^2 P / (h^2 P + r) + q (predicted covariance);
|
|
326
|
+
K = h P/(h^2 P + r) is the steady gain; P_apost = (1 - K h) P is
|
|
327
|
+
the corrected covariance after the update."""
|
|
328
|
+
ff, hh, qq, rr = float(f), float(h), float(q), float(r)
|
|
329
|
+
if hh == 0.0:
|
|
330
|
+
raise ValueError("measurement matrix h must be nonzero")
|
|
331
|
+
if qq < 0.0:
|
|
332
|
+
raise ValueError("process noise q must be >= 0")
|
|
333
|
+
if rr <= 0.0:
|
|
334
|
+
raise ValueError("measurement noise r must be > 0")
|
|
335
|
+
b = rr * (1.0 - ff * ff) - qq * hh * hh
|
|
336
|
+
disc = b * b + 4.0 * hh * hh * qq * rr
|
|
337
|
+
p_inf = (-b + math.sqrt(disc)) / (2.0 * hh * hh)
|
|
338
|
+
k_gain = hh * p_inf / (hh * hh * p_inf + rr)
|
|
339
|
+
p_apost = (1.0 - k_gain * hh) * p_inf
|
|
340
|
+
innov_var = hh * hh * p_inf + rr
|
|
341
|
+
return {"p_pred": p_inf, "gain": k_gain, "p_apost": p_apost,
|
|
342
|
+
"innovation_variance": innov_var}
|
|
343
|
+
|
|
344
|
+
|
|
345
|
+
def observer_gain_2state(A, C, poles) -> dict:
|
|
346
|
+
"""Full-order observer gain L (Ackermann) for a two-state plant.
|
|
347
|
+
|
|
348
|
+
L = phi(A) O^-1 e_2 with phi(s) = (s - p1)(s - p2), O the
|
|
349
|
+
observability matrix [C; C A]. Closed form for the 2x2 case.
|
|
350
|
+
Observer poles must be strictly stable (negative real part).
|
|
351
|
+
"""
|
|
352
|
+
_TOL = 1e-9
|
|
353
|
+
try:
|
|
354
|
+
a00, a01 = float(A[0][0]), float(A[0][1])
|
|
355
|
+
a10, a11 = float(A[1][0]), float(A[1][1])
|
|
356
|
+
c0, c1 = float(C[0][0]), float(C[0][1])
|
|
357
|
+
except (TypeError, ValueError, IndexError) as exc:
|
|
358
|
+
raise ValueError("invalid 2-state observer data: %s" % exc)
|
|
359
|
+
# observability matrix O = [[c0, c1], [c0*a00+c1*a10, c0*a01+c1*a11]]
|
|
360
|
+
o00 = c0
|
|
361
|
+
o01 = c1
|
|
362
|
+
o10 = c0 * a00 + c1 * a10
|
|
363
|
+
o11 = c0 * a01 + c1 * a11
|
|
364
|
+
det_o = o00 * o11 - o01 * o10
|
|
365
|
+
if abs(det_o) <= _TOL:
|
|
366
|
+
raise ValueError("pair (A, C) is not observable (singular O)")
|
|
367
|
+
p1 = complex(poles[0])
|
|
368
|
+
p2 = complex(poles[1])
|
|
369
|
+
if p1.imag != 0.0 or p2.imag != 0.0:
|
|
370
|
+
raise ValueError("2-state closed form expects real poles")
|
|
371
|
+
if p1.real >= 0.0 or p2.real >= 0.0:
|
|
372
|
+
raise ValueError("observer poles must be strictly stable")
|
|
373
|
+
# phi(A) = A^2 - (p1+p2) A + p1 p2 I
|
|
374
|
+
s_p = p1.real + p2.real
|
|
375
|
+
prod = p1.real * p2.real
|
|
376
|
+
A2 = [[a00 * a00 + a01 * a10, a00 * a01 + a01 * a11],
|
|
377
|
+
[a10 * a00 + a11 * a10, a10 * a01 + a11 * a11]]
|
|
378
|
+
phiA = [[A2[0][0] - s_p * a00 + prod, A2[0][1] - s_p * a01],
|
|
379
|
+
[A2[1][0] - s_p * a10, A2[1][1] - s_p * a11 + prod]]
|
|
380
|
+
# O^-1 e2 = last column of O^-1
|
|
381
|
+
inv_last = [-o01 / det_o, o00 / det_o]
|
|
382
|
+
l1 = phiA[0][0] * inv_last[0] + phiA[0][1] * inv_last[1]
|
|
383
|
+
l2 = phiA[1][0] * inv_last[0] + phiA[1][1] * inv_last[1]
|
|
384
|
+
ts = 4.0 / min(abs(p1.real), abs(p2.real))
|
|
385
|
+
return {"L": [l1, l2], "poles": [p1.real, p2.real],
|
|
386
|
+
"settling_time_s": ts, "observable": True}
|
|
387
|
+
|
|
388
|
+
|
|
389
|
+
def pn_acceleration(rx: float, ry: float, vx: float, vy: float,
|
|
390
|
+
n_nav: float) -> dict:
|
|
391
|
+
"""Proportional navigation command a_c = N * Vc * lam_dot (m/s^2).
|
|
392
|
+
|
|
393
|
+
Vc = -(rx vx + ry vy)/r (m/s), lam_dot = (rx vy - ry vx)/r^2
|
|
394
|
+
(rad/s), r = sqrt(rx^2 + ry^2)."""
|
|
395
|
+
rx, ry, vx, vy = (float(v) for v in (rx, ry, vx, vy))
|
|
396
|
+
n = float(n_nav)
|
|
397
|
+
rng = math.hypot(rx, ry)
|
|
398
|
+
if rng <= 1e-12:
|
|
399
|
+
raise ValueError("range must be > 0")
|
|
400
|
+
if n <= 0.0:
|
|
401
|
+
raise ValueError("n_nav must be > 0 (typical N between 3 and 5)")
|
|
402
|
+
vc = -(rx * vx + ry * vy) / rng
|
|
403
|
+
lam_dot = (rx * vy - ry * vx) / (rng * rng)
|
|
404
|
+
return {"range_m": rng, "closing_speed_m_s": vc,
|
|
405
|
+
"los_rate_rad_s": lam_dot, "n_nav": n,
|
|
406
|
+
"accel_cmd_m_s2": n * vc * lam_dot}
|
|
407
|
+
|
|
408
|
+
|
|
409
|
+
def discrete_pid_velocity(Kp: float, Ki: float, Kd: float, T: float) -> dict:
|
|
410
|
+
"""Velocity-form discrete PID coefficients {b0, b1, b2, a1}:
|
|
411
|
+
u(k) = u(k-1) + b0 e(k) + b1 e(k-1) + b2 e(k-2), with
|
|
412
|
+
b0 = Kp + Ki*T + Kd/T, b1 = -Kp - 2*Kd/T, b2 = Kd/T, a1 = -1."""
|
|
413
|
+
T = float(T)
|
|
414
|
+
if T <= 0.0:
|
|
415
|
+
raise ValueError("sample period T must be > 0")
|
|
416
|
+
b0 = Kp + Ki * T + Kd / T
|
|
417
|
+
b1 = -Kp - 2.0 * Kd / T
|
|
418
|
+
b2 = Kd / T
|
|
419
|
+
return {"b0": b0, "b1": b1, "b2": b2, "a1": -1.0}
|
|
420
|
+
|
|
421
|
+
|
|
422
|
+
def sample_rate_rule(wb: float, T: float) -> dict:
|
|
423
|
+
"""Minimum sample-rate rule: sample 10-20 times per closed-loop
|
|
424
|
+
cycle, w_s_min = 10*wb, T_max = 2*pi/w_s_min. Verdict 'ok' when
|
|
425
|
+
T <= T_max."""
|
|
426
|
+
wb, T = float(wb), float(T)
|
|
427
|
+
if wb <= 0.0 or T <= 0.0:
|
|
428
|
+
raise ValueError("wb and T must be > 0")
|
|
429
|
+
w_s_min = 10.0 * wb
|
|
430
|
+
t_max = 2.0 * math.pi / w_s_min
|
|
431
|
+
return {"w_s_min_rad_s": w_s_min, "t_max_s": t_max,
|
|
432
|
+
"verdict": "ok" if T <= t_max else "too-slow"}
|
|
433
|
+
|
|
434
|
+
|
|
435
|
+
def monte_carlo_outer_margins(a_nom: float, kp_nom: float, draws: int,
|
|
436
|
+
seed: int, a_tol: float = 0.15,
|
|
437
|
+
kp_tol: float = 0.10) -> dict:
|
|
438
|
+
"""Seeded Monte Carlo of the outer (type-1) loop margins.
|
|
439
|
+
|
|
440
|
+
Draws a in [a_nom(1-tol), a_nom(1+tol)] and kp in
|
|
441
|
+
[kp_nom(1-tol), kp_nom(1+tol)] uniformly and evaluates the closed
|
|
442
|
+
form margins and damping each draw. Deterministic (fixed seed)."""
|
|
443
|
+
rng = random.Random(seed)
|
|
444
|
+
pm_min, pm_sum, pm_max = float("inf"), 0.0, float("-inf")
|
|
445
|
+
bw_min = float("inf")
|
|
446
|
+
z_min, z_max = float("inf"), float("-inf")
|
|
447
|
+
n_pass = 0
|
|
448
|
+
for _ in range(int(draws)):
|
|
449
|
+
a = a_nom * (1.0 + a_tol * (2.0 * rng.random() - 1.0))
|
|
450
|
+
kp = kp_nom * (1.0 + kp_tol * (2.0 * rng.random() - 1.0))
|
|
451
|
+
K = kp * a
|
|
452
|
+
m = type1_margins(a, K)
|
|
453
|
+
pm = m["phase_margin_deg"]
|
|
454
|
+
wn = math.sqrt(K)
|
|
455
|
+
z = a / (2.0 * wn)
|
|
456
|
+
pm_min = min(pm_min, pm)
|
|
457
|
+
pm_max = max(pm_max, pm)
|
|
458
|
+
pm_sum += pm
|
|
459
|
+
bw = closed_loop_bandwidth(wn, min(z, 1.0))
|
|
460
|
+
bw_min = min(bw_min, bw)
|
|
461
|
+
z_min, z_max = min(z_min, z), max(z_max, z)
|
|
462
|
+
if pm >= 45.0:
|
|
463
|
+
n_pass += 1
|
|
464
|
+
return {"draws": int(draws), "seed": int(seed),
|
|
465
|
+
"a_tol_pct": 100.0 * a_tol, "kp_tol_pct": 100.0 * kp_tol,
|
|
466
|
+
"pm_min_deg": pm_min, "pm_mean_deg": pm_sum / int(draws),
|
|
467
|
+
"pm_max_deg": pm_max, "bandwidth_min_rad_s": bw_min,
|
|
468
|
+
"zeta_min": z_min, "zeta_max": z_max,
|
|
469
|
+
"passes_ge_45": n_pass,
|
|
470
|
+
"all_pass": n_pass == int(draws)}
|
|
471
|
+
|
|
472
|
+
|
|
473
|
+
# ---------------------------------------------------------------------------
|
|
474
|
+
# Project facts
|
|
475
|
+
# ---------------------------------------------------------------------------
|
|
476
|
+
|
|
477
|
+
@dataclass
|
|
478
|
+
class GncProject:
|
|
479
|
+
"""Vehicle/project facts the role needs to build the report."""
|
|
480
|
+
vehicle: str = "Utility UAV (example cruise condition)"
|
|
481
|
+
mission: str = "Pitch attitude hold autopilot design"
|
|
482
|
+
condition: str = "Cruise, 60 m/s, altitude 3000 m, level flight"
|
|
483
|
+
report_title: str = "GNC Design Report - Pitch Attitude Hold Autopilot"
|
|
484
|
+
# plant (short-period reduced order, elevator to pitch rate)
|
|
485
|
+
short_period_wn_rad_s: float = 6.0
|
|
486
|
+
short_period_zeta: float = 0.55
|
|
487
|
+
elevator_effectiveness: float = 4.0 # b_q, 1/s
|
|
488
|
+
# inner loop design targets
|
|
489
|
+
inner_wn_rad_s: float = 12.0
|
|
490
|
+
inner_zeta: float = 0.9
|
|
491
|
+
inner_p3_rad_s: float = 24.0 # non-dominant closed pole
|
|
492
|
+
# outer loop design target
|
|
493
|
+
outer_zeta_target: float = _SQRT2 / 2.0 # 0.707 -> f(z)=1
|
|
494
|
+
# requirements
|
|
495
|
+
req_bandwidth_rad_s: float = 3.0
|
|
496
|
+
req_phase_margin_deg: float = 45.0
|
|
497
|
+
req_gain_margin_db: float = 6.0
|
|
498
|
+
# navigation facts (attitude estimation filter, 100 Hz)
|
|
499
|
+
nav_dt_s: float = 0.01
|
|
500
|
+
nav_process_var: float = 0.0009 # deg^2 per step (gyro drift)
|
|
501
|
+
nav_meas_var: float = 1.0 # deg^2 (accel-derived pitch)
|
|
502
|
+
nav_budget_deg: float = 0.5 # 1-sigma position budget
|
|
503
|
+
# observer (comparison state feedback) facts
|
|
504
|
+
obs_plant_damping: float = 2.0 # theta_ddot = -2 q + u model
|
|
505
|
+
obs_pole_rad_s: float = 30.0 # double observer pole
|
|
506
|
+
# lqr facts (canonical normalized model)
|
|
507
|
+
lqr_damping_a: float = 2.0
|
|
508
|
+
lqr_q1: float = 10.0
|
|
509
|
+
lqr_q2: float = 1.0
|
|
510
|
+
lqr_r: float = 0.1
|
|
511
|
+
# guidance example facts (terminal approach geometry)
|
|
512
|
+
guide_rx_m: float = 2000.0
|
|
513
|
+
guide_ry_m: float = 500.0
|
|
514
|
+
guide_vx_m_s: float = -180.0
|
|
515
|
+
guide_vy_m_s: float = -30.0
|
|
516
|
+
guide_n_nav: float = 4.0
|
|
517
|
+
guide_heading_rad: float = 0.20
|
|
518
|
+
guide_target_speed_m_s: float = 0.0
|
|
519
|
+
# digital implementation
|
|
520
|
+
sample_period_s: float = 0.01
|
|
521
|
+
# Monte Carlo
|
|
522
|
+
mc_draws: int = 400
|
|
523
|
+
mc_seed: int = 7
|
|
524
|
+
mc_a_tol: float = 0.15
|
|
525
|
+
mc_kp_tol: float = 0.10
|
|
526
|
+
|
|
527
|
+
|
|
528
|
+
# ---------------------------------------------------------------------------
|
|
529
|
+
# Report builder
|
|
530
|
+
# ---------------------------------------------------------------------------
|
|
531
|
+
|
|
532
|
+
def build_gnc_report(p: GncProject) -> dict:
|
|
533
|
+
"""Run every design computation and assemble the report model."""
|
|
534
|
+
# plant coefficients: G_q(s) = b/(s^2 + a1 s + a0)
|
|
535
|
+
a1 = 2.0 * p.short_period_zeta * p.short_period_wn_rad_s
|
|
536
|
+
a0 = p.short_period_wn_rad_s ** 2
|
|
537
|
+
b = p.elevator_effectiveness
|
|
538
|
+
|
|
539
|
+
# inner rate loop: PID pole placement
|
|
540
|
+
kp, ki, kd = pid_gains_second_order(
|
|
541
|
+
a1, a0, b, p.inner_wn_rad_s, p.inner_zeta, p.inner_p3_rad_s)
|
|
542
|
+
# inner open loop L(s) = b (kd s^2 + kp s + ki) / (s(s^2 + a1 s + a0))
|
|
543
|
+
inum = [b * kd, b * kp, b * ki]
|
|
544
|
+
iden = [1.0, a1, a0, 0.0]
|
|
545
|
+
inner_m = loop_margins(inum, iden)
|
|
546
|
+
inner_gm_ok = (inner_m["gain_margin_db"] > 0.0
|
|
547
|
+
if not math.isinf(inner_m["gain_margin_db"]) else True)
|
|
548
|
+
inner_pm_ok = (inner_m["phase_margin_deg"] > 0.0
|
|
549
|
+
if not math.isinf(inner_m["phase_margin_deg"]) else True)
|
|
550
|
+
inner_req_ok = (inner_m["phase_margin_deg"] >= p.req_phase_margin_deg
|
|
551
|
+
and inner_gm_ok
|
|
552
|
+
and (inner_m["gain_margin_db"] >= p.req_gain_margin_db
|
|
553
|
+
or math.isinf(inner_m["gain_margin_db"])))
|
|
554
|
+
|
|
555
|
+
# outer attitude loop: reduced inner loop ~ lag a/(s+a), a = zeta_i wn_i
|
|
556
|
+
a_lag = p.inner_zeta * p.inner_wn_rad_s
|
|
557
|
+
zt = p.outer_zeta_target
|
|
558
|
+
wn_theta = a_lag / (2.0 * zt) # from damping target
|
|
559
|
+
kp_theta = wn_theta ** 2 / a_lag # omega_n^2 = kp * a_lag
|
|
560
|
+
outer_m = type1_margins(a_lag, kp_theta * a_lag)
|
|
561
|
+
bw_theta = closed_loop_bandwidth(wn_theta, zt)
|
|
562
|
+
zeta_theta = a_lag / (2.0 * wn_theta)
|
|
563
|
+
outer_req_ok = (outer_m["phase_margin_deg"] >= p.req_phase_margin_deg
|
|
564
|
+
and bw_theta >= p.req_bandwidth_rad_s)
|
|
565
|
+
|
|
566
|
+
# digital
|
|
567
|
+
dig = sample_rate_rule(p.inner_wn_rad_s, p.sample_period_s)
|
|
568
|
+
disc = discrete_pid_velocity(kp, ki, kd, p.sample_period_s)
|
|
569
|
+
|
|
570
|
+
# nav (attitude estimation Kalman, f = h = 1 model)
|
|
571
|
+
nav = kalman_steady_state(1.0, 1.0, p.nav_process_var, p.nav_meas_var)
|
|
572
|
+
nav_sigma_deg = math.sqrt(nav["p_apost"])
|
|
573
|
+
nav_ok = nav_sigma_deg <= p.nav_budget_deg
|
|
574
|
+
|
|
575
|
+
# observer (comparison state-feedback inner design)
|
|
576
|
+
obs_A = [[0.0, 1.0], [0.0, -p.obs_plant_damping]]
|
|
577
|
+
obs_C = [[1.0, 0.0]]
|
|
578
|
+
obs = observer_gain_2state(obs_A, obs_C,
|
|
579
|
+
[-p.obs_pole_rad_s, -p.obs_pole_rad_s])
|
|
580
|
+
|
|
581
|
+
# LQR comparison
|
|
582
|
+
lqr_A = [[0.0, 1.0], [0.0, -p.lqr_damping_a]]
|
|
583
|
+
lqr_B = [0.0, 1.0]
|
|
584
|
+
lqr_Q = [[p.lqr_q1, 0.0], [0.0, p.lqr_q2]]
|
|
585
|
+
lqr = riccati_solution(lqr_A, lqr_B, lqr_Q, p.lqr_r)
|
|
586
|
+
|
|
587
|
+
# guidance (PN comparison for the same geometry)
|
|
588
|
+
guide = pn_acceleration(p.guide_rx_m, p.guide_ry_m, p.guide_vx_m_s,
|
|
589
|
+
p.guide_vy_m_s, p.guide_n_nav)
|
|
590
|
+
los_deg = math.degrees(math.atan2(p.guide_ry_m, p.guide_rx_m))
|
|
591
|
+
heading_error_deg = math.degrees(
|
|
592
|
+
math.atan2(math.sin(los_deg * math.pi / 180.0 - p.guide_heading_rad),
|
|
593
|
+
math.cos(los_deg * math.pi / 180.0 - p.guide_heading_rad)))
|
|
594
|
+
vi = math.hypot(p.guide_vx_m_s, p.guide_vy_m_s)
|
|
595
|
+
vt = p.guide_target_speed_m_s
|
|
596
|
+
capture = vi > vt
|
|
597
|
+
t_i = guide["range_m"] / (vi - vt) if capture else None
|
|
598
|
+
|
|
599
|
+
# Monte Carlo robustness on the outer loop
|
|
600
|
+
mc = monte_carlo_outer_margins(a_lag, kp_theta, p.mc_draws, p.mc_seed,
|
|
601
|
+
p.mc_a_tol, p.mc_kp_tol)
|
|
602
|
+
mc_ok = mc["all_pass"]
|
|
603
|
+
|
|
604
|
+
model = {
|
|
605
|
+
"document_type": p.report_title,
|
|
606
|
+
"status": "draft-for-review",
|
|
607
|
+
"vehicle": p.vehicle,
|
|
608
|
+
"mission": p.mission,
|
|
609
|
+
"condition": p.condition,
|
|
610
|
+
"generated": _today(),
|
|
611
|
+
# plant
|
|
612
|
+
"plant": {
|
|
613
|
+
"wn_sp": p.short_period_wn_rad_s,
|
|
614
|
+
"zeta_sp": p.short_period_zeta,
|
|
615
|
+
"a1": a1, "a0": a0, "b": b,
|
|
616
|
+
"model": "%.1f / (s^2 + %.1f s + %.1f)"
|
|
617
|
+
% (b, a1, a0),
|
|
618
|
+
},
|
|
619
|
+
# inner loop
|
|
620
|
+
"inner": {
|
|
621
|
+
"wn": p.inner_wn_rad_s, "zeta": p.inner_zeta,
|
|
622
|
+
"p3": p.inner_p3_rad_s,
|
|
623
|
+
"kp": kp, "ki": ki, "kd": kd,
|
|
624
|
+
"margins": {
|
|
625
|
+
"gm_db": inner_m["gain_margin_db"],
|
|
626
|
+
"pm_deg": inner_m["phase_margin_deg"],
|
|
627
|
+
"wgc": inner_m["gain_crossover_rad_s"],
|
|
628
|
+
"wpc": inner_m["phase_crossover_rad_s"],
|
|
629
|
+
},
|
|
630
|
+
"req_ok": bool(inner_req_ok),
|
|
631
|
+
},
|
|
632
|
+
# outer loop
|
|
633
|
+
"outer": {
|
|
634
|
+
"a_lag": a_lag, "kp_theta": kp_theta,
|
|
635
|
+
"wn_theta": wn_theta, "zeta_theta": zeta_theta,
|
|
636
|
+
"bw_rad_s": bw_theta,
|
|
637
|
+
"margins": {"gm_db": outer_m["gain_margin_db"],
|
|
638
|
+
"pm_deg": outer_m["phase_margin_deg"],
|
|
639
|
+
"wgc": outer_m["crossover_rad_s"]},
|
|
640
|
+
"req_ok": bool(outer_req_ok),
|
|
641
|
+
},
|
|
642
|
+
"digital": {
|
|
643
|
+
"T_s": p.sample_period_s,
|
|
644
|
+
"w_s_min": dig["w_s_min_rad_s"],
|
|
645
|
+
"t_max_s": dig["t_max_s"],
|
|
646
|
+
"verdict": dig["verdict"],
|
|
647
|
+
"disc": disc,
|
|
648
|
+
},
|
|
649
|
+
"nav": {
|
|
650
|
+
"dt_s": p.nav_dt_s,
|
|
651
|
+
"q_deg2": p.nav_process_var,
|
|
652
|
+
"r_deg2": p.nav_meas_var,
|
|
653
|
+
"p_pred": nav["p_pred"],
|
|
654
|
+
"gain": nav["gain"],
|
|
655
|
+
"p_apost": nav["p_apost"],
|
|
656
|
+
"sigma_deg": nav_sigma_deg,
|
|
657
|
+
"budget_deg": p.nav_budget_deg,
|
|
658
|
+
"innovation_sigma_deg": math.sqrt(nav["innovation_variance"]),
|
|
659
|
+
"ok": bool(nav_ok),
|
|
660
|
+
},
|
|
661
|
+
"observer": {
|
|
662
|
+
"plant_note": "theta_ddot = -%g q + u (normalized elevator "
|
|
663
|
+
"pitch acceleration)" % p.obs_plant_damping,
|
|
664
|
+
"L": obs["L"], "poles": obs["poles"],
|
|
665
|
+
"settling_time_s": obs["settling_time_s"],
|
|
666
|
+
},
|
|
667
|
+
"lqr": {
|
|
668
|
+
"a_d": p.lqr_damping_a, "q1": p.lqr_q1, "q2": p.lqr_q2,
|
|
669
|
+
"r": p.lqr_r,
|
|
670
|
+
"P": lqr["P"], "K": lqr["K"], "stable": lqr["stable"],
|
|
671
|
+
"poles": lqr["poles"],
|
|
672
|
+
},
|
|
673
|
+
"guidance": {
|
|
674
|
+
"range_m": guide["range_m"],
|
|
675
|
+
"closing_speed_m_s": guide["closing_speed_m_s"],
|
|
676
|
+
"los_rate_rad_s": guide["los_rate_rad_s"],
|
|
677
|
+
"n_nav": guide["n_nav"],
|
|
678
|
+
"accel_cmd_m_s2": guide["accel_cmd_m_s2"],
|
|
679
|
+
"los_deg": los_deg,
|
|
680
|
+
"heading_rad": p.guide_heading_rad,
|
|
681
|
+
"heading_error_deg": heading_error_deg,
|
|
682
|
+
"capture_possible": capture,
|
|
683
|
+
"intercept_time_s": t_i,
|
|
684
|
+
"interceptor_speed_m_s": vi,
|
|
685
|
+
},
|
|
686
|
+
"mc": mc,
|
|
687
|
+
"mc_ok": bool(mc_ok),
|
|
688
|
+
# requirement record (for gates and conclusions)
|
|
689
|
+
"requirements": {
|
|
690
|
+
"bandwidth_rad_s": p.req_bandwidth_rad_s,
|
|
691
|
+
"phase_margin_deg": p.req_phase_margin_deg,
|
|
692
|
+
"gain_margin_db": p.req_gain_margin_db,
|
|
693
|
+
},
|
|
694
|
+
# verdict rows for the conclusions table
|
|
695
|
+
"verdicts": {
|
|
696
|
+
"inner_margins": "PASS" if inner_req_ok else "FAIL",
|
|
697
|
+
"outer_margins": "PASS" if outer_req_ok else "FAIL",
|
|
698
|
+
"outer_bandwidth": "PASS" if bw_theta >= p.req_bandwidth_rad_s
|
|
699
|
+
else "FAIL",
|
|
700
|
+
"nav_budget": "PASS" if nav_ok else "FAIL",
|
|
701
|
+
"mc_robustness": "PASS" if mc_ok else "FAIL",
|
|
702
|
+
},
|
|
703
|
+
}
|
|
704
|
+
return model
|
|
705
|
+
|
|
706
|
+
|
|
707
|
+
# ---------------------------------------------------------------------------
|
|
708
|
+
# Markdown rendering
|
|
709
|
+
# ---------------------------------------------------------------------------
|
|
710
|
+
|
|
711
|
+
def _fmt(x, nd=3):
|
|
712
|
+
if isinstance(x, float) and math.isinf(x):
|
|
713
|
+
return "inf"
|
|
714
|
+
return ("%%.%df" % nd) % x
|
|
715
|
+
|
|
716
|
+
|
|
717
|
+
def render_gnc_report_markdown(m: dict) -> str:
|
|
718
|
+
"""Render the report content model as the deliverable markdown."""
|
|
719
|
+
p = m["plant"]
|
|
720
|
+
i = m["inner"]
|
|
721
|
+
o = m["outer"]
|
|
722
|
+
d = m["digital"]
|
|
723
|
+
n = m["nav"]
|
|
724
|
+
ob = m["observer"]
|
|
725
|
+
lq = m["lqr"]
|
|
726
|
+
g = m["guidance"]
|
|
727
|
+
mc = m["mc"]
|
|
728
|
+
req = m["requirements"]
|
|
729
|
+
v = m["verdicts"]
|
|
730
|
+
gm_in = i["margins"]["gm_db"]
|
|
731
|
+
pm_in = i["margins"]["pm_deg"]
|
|
732
|
+
L = ob["L"]
|
|
733
|
+
lines = [
|
|
734
|
+
"# " + m["document_type"],
|
|
735
|
+
"",
|
|
736
|
+
"**Vehicle:** %s" % m["vehicle"],
|
|
737
|
+
"**Condition:** %s" % m["condition"],
|
|
738
|
+
"**Mission / item:** %s" % m["mission"],
|
|
739
|
+
"**Status:** %s" % m["status"],
|
|
740
|
+
"",
|
|
741
|
+
"## 1. Architecture and plant",
|
|
742
|
+
"",
|
|
743
|
+
"- Control architecture: cascade pitch attitude hold - inner pitch "
|
|
744
|
+
"rate (q) loop with PID; outer pitch attitude (theta) loop "
|
|
745
|
+
"(proportional). Guidance and navigation layers analyzed in "
|
|
746
|
+
"sections 2 and 6.",
|
|
747
|
+
"- Frames: body-fixed axes (x forward, z down) for the short-period "
|
|
748
|
+
"model; stability axes for the longitudinal modes; inertial NED for "
|
|
749
|
+
"the guidance geometry.",
|
|
750
|
+
"- Plant (reduced-order short-period, elevator to pitch rate, at the "
|
|
751
|
+
"stated cruise condition):",
|
|
752
|
+
"",
|
|
753
|
+
" G_q(s) = %s" % p["model"],
|
|
754
|
+
"",
|
|
755
|
+
" with short-period natural frequency %.1f rad/s, damping %.2f "
|
|
756
|
+
"and elevator effectiveness %.1f 1/s (example trim data; "
|
|
757
|
+
"verify against the vehicle aerodynamic database)." % (
|
|
758
|
+
p["wn_sp"], p["zeta_sp"], p["b"]),
|
|
759
|
+
"- States: pitch rate q (plant state; the short-period model "
|
|
760
|
+
"implicitly includes angle of attack). Input: elevator deflection. "
|
|
761
|
+
"Output: pitch rate q. Pitch attitude theta = q/s for the outer "
|
|
762
|
+
"loop.",
|
|
763
|
+
"- Assumptions: rigid airframe (no aeroelastic modes in this band), "
|
|
764
|
+
"actuator modeled at >= 40 rad/s in the follow-up stage.",
|
|
765
|
+
"",
|
|
766
|
+
"## 2. Navigation",
|
|
767
|
+
"",
|
|
768
|
+
"- Sensor suite: rate gyro (pitch axis) and accelerometer-derived "
|
|
769
|
+
"pitch measurement; scalar attitude Kalman filter at %d Hz."
|
|
770
|
+
% int(round(1.0 / n["dt_s"])),
|
|
771
|
+
"- Noise models: process noise q = %.4f deg^2 per step (integrated "
|
|
772
|
+
"gyro drift), measurement noise r = %.2f deg^2 (accel-derived "
|
|
773
|
+
"pitch, non-accelerated-flight assumption)." % (n["q_deg2"],
|
|
774
|
+
n["r_deg2"]),
|
|
775
|
+
"- Filter: constant model f = 1, h = 1. Steady-state predicted "
|
|
776
|
+
"covariance P = %.4f deg^2; steady-state gain K = %.4f; "
|
|
777
|
+
"post-update covariance P+ = %.4f deg^2." % (
|
|
778
|
+
n["p_pred"], n["gain"], n["p_apost"]),
|
|
779
|
+
"- Attitude (pitch) error budget: 1-sigma = %.3f deg against a "
|
|
780
|
+
"budget of %.1f deg - %s." % (
|
|
781
|
+
n["sigma_deg"], n["budget_deg"],
|
|
782
|
+
"MET" if n["ok"] else "NOT MET"),
|
|
783
|
+
"- Innovation monitor: 1-sigma innovation = %.3f deg."
|
|
784
|
+
% n["innovation_sigma_deg"],
|
|
785
|
+
"- GNSS/RAIM: not part of this attitude channel; the "
|
|
786
|
+
"gnss-pseudorange-positioning / gnss-raim-fde / "
|
|
787
|
+
"dilution-of-precision leaves apply when a full position "
|
|
788
|
+
"navigation solution is in scope.",
|
|
789
|
+
"",
|
|
790
|
+
"## 3. Control design",
|
|
791
|
+
"",
|
|
792
|
+
"Inner pitch-rate loop (PID by pole placement on the short-period "
|
|
793
|
+
"plant, matched to (s^2 + 2 z_i wn_i s + wn_i^2)(s + p3_i)):",
|
|
794
|
+
"",
|
|
795
|
+
"| Loop | Gains | Phase margin | Gain margin | Requirement | Verdict |",
|
|
796
|
+
"|---|---|---|---|---|---|",
|
|
797
|
+
"| inner q (PID) | kp=%.2f, ki=%.2f, kd=%.2f | %.1f deg | %s dB | "
|
|
798
|
+
"PM >= %.0f deg, GM >= %.0f dB | %s |" % (
|
|
799
|
+
i["kp"], i["ki"], i["kd"],
|
|
800
|
+
(pm_in if not math.isinf(pm_in) else float("inf")),
|
|
801
|
+
("inf" if math.isinf(gm_in) else "%.1f" % gm_in),
|
|
802
|
+
req["phase_margin_deg"], req["gain_margin_db"],
|
|
803
|
+
"PASS" if i["req_ok"] else "FAIL"),
|
|
804
|
+
"| outer theta (P) | kp_theta=%.3f 1/s | %.1f deg | inf dB | "
|
|
805
|
+
"PM >= %.0f deg, GM >= 6 dB | %s |" % (
|
|
806
|
+
o["kp_theta"], o["margins"]["pm_deg"],
|
|
807
|
+
req["phase_margin_deg"],
|
|
808
|
+
"PASS" if o["req_ok"] else "FAIL"),
|
|
809
|
+
"",
|
|
810
|
+
"Inner loop design: wn_i = %.1f rad/s (>= 3x the attitude "
|
|
811
|
+
"bandwidth requirement of %.1f rad/s), zeta_i = %.2f, "
|
|
812
|
+
"non-dominant pole p3_i = %.1f rad/s. Inner open loop "
|
|
813
|
+
"L(s) = C(s) G_q(s): gain crossover %.1f rad/s, phase crossover %s, "
|
|
814
|
+
"phase margin %.1f deg, gain margin %s dB (type-1 loop, phase "
|
|
815
|
+
"recovers above -180 deg so the gain margin is infinite)." % (
|
|
816
|
+
i["wn"], req["bandwidth_rad_s"], i["zeta"], i["p3"],
|
|
817
|
+
i["margins"]["wgc"],
|
|
818
|
+
("none" if math.isinf(i["margins"]["wpc"])
|
|
819
|
+
else "%.1f rad/s" % i["margins"]["wpc"]),
|
|
820
|
+
pm_in, gm_in),
|
|
821
|
+
"",
|
|
822
|
+
"Outer loop: the closed inner loop is reduced to the lag "
|
|
823
|
+
"a/(s + a) with a = zeta_i * wn_i = %.1f rad/s, so the outer "
|
|
824
|
+
"attitude plant is a/(s(s + a)) (type-1). The proportional gain "
|
|
825
|
+
"kp_theta is sized for damping zeta = %.2f "
|
|
826
|
+
"(f(z) = 1 at z = 0.707, so bandwidth = natural frequency):" % (
|
|
827
|
+
o["a_lag"], o["zeta_theta"]),
|
|
828
|
+
"",
|
|
829
|
+
"- Natural frequency from the bandwidth requirement: "
|
|
830
|
+
"wn,req = w_bw / f(zeta) = %.1f / 1.000 = %.1f rad/s. The "
|
|
831
|
+
"natural frequency implied by the damping target "
|
|
832
|
+
"(wn = a / (2 zeta)) is %.2f rad/s and exceeds the requirement."
|
|
833
|
+
% (req["bandwidth_rad_s"], req["bandwidth_rad_s"],
|
|
834
|
+
o["wn_theta"]),
|
|
835
|
+
"- Achieved: wn_theta = %.2f rad/s, zeta_theta = %.2f, "
|
|
836
|
+
"bandwidth w_bw = %.2f rad/s >= %.1f rad/s requirement (%s), "
|
|
837
|
+
"phase margin %.1f deg >= %.0f deg (%s), gain margin inf dB "
|
|
838
|
+
">= 6 dB (%s)." % (
|
|
839
|
+
o["wn_theta"], o["zeta_theta"], o["bw_rad_s"],
|
|
840
|
+
req["bandwidth_rad_s"],
|
|
841
|
+
"PASS" if o["bw_rad_s"] >= req["bandwidth_rad_s"] else "FAIL",
|
|
842
|
+
o["margins"]["pm_deg"], req["phase_margin_deg"],
|
|
843
|
+
"PASS" if o["margins"]["pm_deg"] >= req["phase_margin_deg"]
|
|
844
|
+
else "FAIL",
|
|
845
|
+
"PASS"),
|
|
846
|
+
"",
|
|
847
|
+
"## 4. Digital implementation",
|
|
848
|
+
"",
|
|
849
|
+
"- Sample period T = %.3f s (%d Hz); closed-loop characteristic "
|
|
850
|
+
"frequency wn_i = %.1f rad/s." % (d["T_s"], int(round(1.0 / d["T_s"])),
|
|
851
|
+
i["wn"]),
|
|
852
|
+
"- Sample-rate rule (10-20 samples per closed-loop cycle): "
|
|
853
|
+
"w_s,min = 10 * wn_i = %.1f rad/s, T_max = %.4f s; verdict: %s "
|
|
854
|
+
"(T = %.3f s <= T_max)." % (
|
|
855
|
+
d["w_s_min"], d["t_max_s"], d["verdict"], d["T_s"]),
|
|
856
|
+
"- Inner PID velocity-form coefficients at T = %.3f s "
|
|
857
|
+
"(u(k) = u(k-1) + b0 e(k) + b1 e(k-1) + b2 e(k-2)): "
|
|
858
|
+
"b0 = %.4f, b1 = %.4f, b2 = %.4f, a1 = -1." % (
|
|
859
|
+
d["T_s"], d["disc"]["b0"], d["disc"]["b1"], d["disc"]["b2"]),
|
|
860
|
+
"",
|
|
861
|
+
"## 5. State estimation (observer)",
|
|
862
|
+
"",
|
|
863
|
+
"- Full-order observer (Luenberger/Ackermann) for the comparison "
|
|
864
|
+
"state-feedback design; plant %s, measured state theta (C = "
|
|
865
|
+
"[1, 0])." % ob["plant_note"],
|
|
866
|
+
"- Observer poles: %.0f rad/s (double) - about 10x faster than the "
|
|
867
|
+
"controller poles (separation principle), settling time "
|
|
868
|
+
"4/sigma = %.3f s." % (ob["poles"][0], ob["settling_time_s"]),
|
|
869
|
+
"- Observer gain L = [%.1f, %.1f]." % (L[0], L[1]),
|
|
870
|
+
"",
|
|
871
|
+
"## 6. Guidance and optimal control",
|
|
872
|
+
"",
|
|
873
|
+
"Guidance (terminal approach example, proportional navigation "
|
|
874
|
+
"comparison; relative state from the nav estimate):",
|
|
875
|
+
"",
|
|
876
|
+
"- Geometry: range r = %.1f m, line-of-sight %.2f deg; heading "
|
|
877
|
+
"error %.2f deg; closing speed Vc = %.2f m/s; line-of-sight rate "
|
|
878
|
+
"%.5f rad/s." % (g["range_m"], g["los_deg"], g["heading_error_deg"],
|
|
879
|
+
g["closing_speed_m_s"], g["los_rate_rad_s"]),
|
|
880
|
+
"- Commanded lateral acceleration a_c = N * Vc * lam_dot = "
|
|
881
|
+
"%.1f * %.1f * %.5f = %.2f m/s^2 (%.2f g) with N = %.1f; pure "
|
|
882
|
+
"pursuit capture possible (V_i = %.1f m/s > V_t = 0), tail-chase "
|
|
883
|
+
"intercept time ~%.1f s." % (
|
|
884
|
+
g["n_nav"], g["closing_speed_m_s"], g["los_rate_rad_s"],
|
|
885
|
+
g["accel_cmd_m_s2"], g["accel_cmd_m_s2"] / 9.80665,
|
|
886
|
+
g["n_nav"], g["interceptor_speed_m_s"], g["intercept_time_s"]),
|
|
887
|
+
"",
|
|
888
|
+
"Optimal control (LQR comparison on the normalized pitch-axis "
|
|
889
|
+
"model x = [theta, q], x_dot = A x + B u with "
|
|
890
|
+
"A = [[0,1],[0,-%.0f]], B = [0,1], u = normalized elevator pitch "
|
|
891
|
+
"acceleration):" % lq["a_d"],
|
|
892
|
+
"",
|
|
893
|
+
"- Weights: Q = diag(%.0f, %.0f) (state error), R = %.1f (control "
|
|
894
|
+
"effort)." % (lq["q1"], lq["q2"], lq["r"]),
|
|
895
|
+
"- Riccati solution P = [[%.3f, %.3f],[%.3f, %.3f]]; gain "
|
|
896
|
+
"K = R^-1 B' P = [%.2f, %.2f]." % (
|
|
897
|
+
lq["P"][0][0], lq["P"][0][1], lq["P"][1][0], lq["P"][1][1],
|
|
898
|
+
lq["K"][0], lq["K"][1]),
|
|
899
|
+
"- Closed loop A - B K stable: %s (poles at -%.2f +/- j%.2f "
|
|
900
|
+
"rad/s, wn = %.2f rad/s, zeta = %.2f). The LQR result is a "
|
|
901
|
+
"comparison design on the same axis; the PID cascade of section 3 "
|
|
902
|
+
"is the baseline." % (
|
|
903
|
+
"yes" if lq["stable"] else "no",
|
|
904
|
+
-lq["poles"][0][0], abs(lq["poles"][0][1]),
|
|
905
|
+
math.sqrt(lq["K"][0]), lq["a_d"] / (2 * math.sqrt(lq["K"][0]))),
|
|
906
|
+
"",
|
|
907
|
+
"## 7. Space GNC",
|
|
908
|
+
"",
|
|
909
|
+
"- Not applicable for this aircraft example. The space leaves "
|
|
910
|
+
"(attitude-dynamics, orbit-dynamics, orbit-determination, "
|
|
911
|
+
"rendezvous-phasing) load when the vehicle is a spacecraft.",
|
|
912
|
+
"",
|
|
913
|
+
"## 8. Monte Carlo / robustness",
|
|
914
|
+
"",
|
|
915
|
+
"- Monte Carlo on the outer loop (%d draws, seed %d): lag a and "
|
|
916
|
+
"gain kp_theta varied uniformly over +/- %.0f%% (a, inner-loop "
|
|
917
|
+
"uncertainty) and +/- %.0f%% (kp_theta, implementation "
|
|
918
|
+
"tolerance)." % (
|
|
919
|
+
mc["draws"], mc["seed"], mc["a_tol_pct"], mc["kp_tol_pct"]),
|
|
920
|
+
"- Result: phase margin min %.1f deg / mean %.1f deg / max %.1f "
|
|
921
|
+
"deg; all %d draws >= 45 deg (%s); zeta_theta range %.2f-%.2f; "
|
|
922
|
+
"minimum bandwidth %.2f rad/s (still >= %.1f rad/s requirement)."
|
|
923
|
+
% (mc["pm_min_deg"], mc["pm_mean_deg"], mc["pm_max_deg"],
|
|
924
|
+
mc["draws"], "PASS" if mc["all_pass"] else "FAIL",
|
|
925
|
+
mc["zeta_min"], mc["zeta_max"], mc["bandwidth_min_rad_s"],
|
|
926
|
+
req["bandwidth_rad_s"]),
|
|
927
|
+
"- Disturbance response: the rate-loop integral gives zero "
|
|
928
|
+
"steady-state attitude error to constant pitch disturbances "
|
|
929
|
+
"(type-1 loop); transient response and gust loads verified in the "
|
|
930
|
+
"full 6-DOF simulation (open item).",
|
|
931
|
+
"",
|
|
932
|
+
"## 9. Conclusions",
|
|
933
|
+
"",
|
|
934
|
+
"| Gate | Result |",
|
|
935
|
+
"|---|---|",
|
|
936
|
+
"| Inner loop margins vs requirement | %s |" % v["inner_margins"],
|
|
937
|
+
"| Outer loop margins vs requirement | %s |" % v["outer_margins"],
|
|
938
|
+
"| Attitude bandwidth >= %.1f rad/s | %s |" % (
|
|
939
|
+
req["bandwidth_rad_s"], v["outer_bandwidth"]),
|
|
940
|
+
"| Nav error budget (1-sigma <= %.1f deg) | %s |" % (
|
|
941
|
+
n["budget_deg"], v["nav_budget"]),
|
|
942
|
+
"| Monte Carlo robustness (PM >= 45 deg) | %s |" % v["mc_robustness"],
|
|
943
|
+
"",
|
|
944
|
+
"- Margins are design predictions from the stated reduced-order "
|
|
945
|
+
"model and sensor assumptions.",
|
|
946
|
+
"- Open items for flight test / formal verification: full 6-DOF "
|
|
947
|
+
"nonlinear simulation with actuator and aeroelastic models, "
|
|
948
|
+
"sensor-in-the-loop tests, gain-schedule coverage of the flight "
|
|
949
|
+
"envelope, and handling-qualities evaluation per MIL-STD-1797A "
|
|
950
|
+
"where applicable.",
|
|
951
|
+
"",
|
|
952
|
+
"## Appendix A. Calculation traceability",
|
|
953
|
+
"",
|
|
954
|
+
"- PID gains: pole placement matching "
|
|
955
|
+
"(s^2 + 2 z wn s + wn^2)(s + p3) (pid-control-design).",
|
|
956
|
+
"- Bandwidth/natural frequency: w_bw = f(z) wn with "
|
|
957
|
+
"f(z) = sqrt(1 - 2z^2 + sqrt(2 - 4z^2 + 4z^4)); f(0.707) = 1.",
|
|
958
|
+
"- Margins: type-1 loop L = K/(s(s + a)) closed form; general "
|
|
959
|
+
"loops evaluated on jw with root-sum unwrapped phase "
|
|
960
|
+
"(frequency-response-design).",
|
|
961
|
+
"- LQR: K = R^-1 B' P, ARE closed form (lqr-design).",
|
|
962
|
+
"- Kalman: steady-state covariance = scalar Riccati root; "
|
|
963
|
+
"K = h P/(h^2 P + r) (kalman-filter-design).",
|
|
964
|
+
"- Observer: Ackermann L = phi(A) O^-1 e_n (observer-design).",
|
|
965
|
+
"- Guidance: a_c = N Vc lam_dot (pursuit-guidance).",
|
|
966
|
+
"- Digital: 10-20 samples per closed-loop cycle; velocity-form "
|
|
967
|
+
"discrete PID (digital-control-design).",
|
|
968
|
+
"",
|
|
969
|
+
"---",
|
|
970
|
+
"*Generated by Aero Agent Roles gnc-engineer core (%s). DRAFT - "
|
|
971
|
+
"for human GNC lead review. Design predictions only; not flight "
|
|
972
|
+
"software release, not a certification finding, not an approval "
|
|
973
|
+
"document.*" % m["generated"],
|
|
974
|
+
]
|
|
975
|
+
return "\n".join(lines)
|
|
976
|
+
|
|
977
|
+
|
|
978
|
+
# ---------------------------------------------------------------------------
|
|
979
|
+
# Evidence gates
|
|
980
|
+
# ---------------------------------------------------------------------------
|
|
981
|
+
|
|
982
|
+
def _margins_pass(m: dict) -> bool:
|
|
983
|
+
req = m["requirements"]
|
|
984
|
+
i = m["inner"]["margins"]
|
|
985
|
+
o = m["outer"]["margins"]
|
|
986
|
+
ipm = i["pm_deg"]
|
|
987
|
+
igm = i["gm_db"]
|
|
988
|
+
return (not math.isinf(ipm) and ipm >= req["phase_margin_deg"]
|
|
989
|
+
and (math.isinf(igm) or igm >= req["gain_margin_db"])
|
|
990
|
+
and not math.isinf(o["pm_deg"])
|
|
991
|
+
and o["pm_deg"] >= req["phase_margin_deg"])
|
|
992
|
+
|
|
993
|
+
|
|
994
|
+
def check_gnc_report(m: dict) -> dict:
|
|
995
|
+
"""Evidence gates against the report content model."""
|
|
996
|
+
req = m["requirements"]
|
|
997
|
+
results = {
|
|
998
|
+
"gains_present": all(
|
|
999
|
+
isinstance(m[k].get(x), (int, float))
|
|
1000
|
+
for k, xs in (("inner", ("kp", "ki", "kd")),
|
|
1001
|
+
("outer", ("kp_theta",))) for x in xs),
|
|
1002
|
+
"margins_meet_requirement": bool(_margins_pass(m)),
|
|
1003
|
+
"bandwidth_meets_requirement": bool(
|
|
1004
|
+
m["outer"]["bw_rad_s"] >= req["bandwidth_rad_s"]),
|
|
1005
|
+
"nav_error_budget_met": bool(m["nav"]["ok"]),
|
|
1006
|
+
"mc_robustness_pass": bool(m["mc_ok"]),
|
|
1007
|
+
"sign_off_honest": m.get("status") == "draft-for-review",
|
|
1008
|
+
}
|
|
1009
|
+
results["all_pass"] = all(results.values())
|
|
1010
|
+
return results
|
|
1011
|
+
|
|
1012
|
+
|
|
1013
|
+
def check_gnc_report_markdown(md_text: str) -> dict:
|
|
1014
|
+
"""Gate-check the rendered markdown deliverable."""
|
|
1015
|
+
low = md_text.lower()
|
|
1016
|
+
checks = {
|
|
1017
|
+
"has_title": "gnc design report" in low,
|
|
1018
|
+
"has_plant": "g_q(s)" in low,
|
|
1019
|
+
"has_gains": bool(__import__("re").search(r"kp\s*=\s*\d", low)),
|
|
1020
|
+
"has_phase_margin": "phase margin" in low,
|
|
1021
|
+
"has_gain_margin": "gain margin" in low,
|
|
1022
|
+
"has_nav_budget": "error budget" in low,
|
|
1023
|
+
"no_blank_fields": "___" not in md_text,
|
|
1024
|
+
"has_draft_marker": "draft" in low,
|
|
1025
|
+
"has_not_approval": "not an approval" in low,
|
|
1026
|
+
}
|
|
1027
|
+
checks["all_pass"] = all(checks.values())
|
|
1028
|
+
return checks
|
|
1029
|
+
|
|
1030
|
+
|
|
1031
|
+
def validate_deliverable_text(md_text: str) -> dict:
|
|
1032
|
+
"""Public entry point used by gate tooling: check a GNC report."""
|
|
1033
|
+
return check_gnc_report_markdown(md_text)
|
|
1034
|
+
|
|
1035
|
+
|
|
1036
|
+
# ---------------------------------------------------------------------------
|
|
1037
|
+
# Example project (tests + worked-example generation)
|
|
1038
|
+
# ---------------------------------------------------------------------------
|
|
1039
|
+
|
|
1040
|
+
def example_project() -> GncProject:
|
|
1041
|
+
return GncProject()
|
|
1042
|
+
|
|
1043
|
+
|
|
1044
|
+
def example_report_markdown() -> str:
|
|
1045
|
+
return render_gnc_report_markdown(build_gnc_report(example_project()))
|
|
1046
|
+
|
|
1047
|
+
|
|
1048
|
+
if __name__ == "__main__":
|
|
1049
|
+
proj = example_project()
|
|
1050
|
+
model = build_gnc_report(proj)
|
|
1051
|
+
md = render_gnc_report_markdown(model)
|
|
1052
|
+
obs_L = model["observer"]["L"]
|
|
1053
|
+
print("VEHICLE: %s" % model["vehicle"])
|
|
1054
|
+
print("PLANT: %s" % model["plant"]["model"])
|
|
1055
|
+
print("INNER PID: kp=%.2f ki=%.2f kd=%.2f | PM=%.1f deg GM=%s dB"
|
|
1056
|
+
% (model["inner"]["kp"], model["inner"]["ki"],
|
|
1057
|
+
model["inner"]["kd"], model["inner"]["margins"]["pm_deg"],
|
|
1058
|
+
model["inner"]["margins"]["gm_db"]))
|
|
1059
|
+
print("OUTER: kp_theta=%.3f | PM=%.1f deg | BW=%.2f rad/s"
|
|
1060
|
+
% (model["outer"]["kp_theta"], model["outer"]["margins"]["pm_deg"],
|
|
1061
|
+
model["outer"]["bw_rad_s"]))
|
|
1062
|
+
print("NAV: 1-sigma=%.3f deg K=%.4f" % (model["nav"]["sigma_deg"],
|
|
1063
|
+
model["nav"]["gain"]))
|
|
1064
|
+
print("GUIDANCE: a_c=%.2f m/s^2" % model["guidance"]["accel_cmd_m_s2"])
|
|
1065
|
+
print("LQR K=[%.2f, %.2f] stable=%s" % (model["lqr"]["K"][0],
|
|
1066
|
+
model["lqr"]["K"][1],
|
|
1067
|
+
model["lqr"]["stable"]))
|
|
1068
|
+
print("OBS L=[%.1f, %.1f]" % (obs_L[0], obs_L[1]))
|
|
1069
|
+
print("MC: PM min=%.1f mean=%.1f max=%.1f all_pass=%s"
|
|
1070
|
+
% (model["mc"]["pm_min_deg"], model["mc"]["pm_mean_deg"],
|
|
1071
|
+
model["mc"]["pm_max_deg"], model["mc"]["all_pass"]))
|
|
1072
|
+
print("GATES(model): %s" % check_gnc_report(model))
|
|
1073
|
+
print("GATES(markdown): %s" % check_gnc_report_markdown(md))
|
|
1074
|
+
print("RENDERED: %d chars" % len(md))
|