import math import random from copy import deepcopy from typing import Optional from uas.model.constants import * from uas.model.sensor_noise import SensorNoiseParams from uas.model.state import AircraftState from uas.utils.geo_utils import GeoUtils # ══════════════════════════════════════════════════════════════════════ # 导航解算器（简化卡尔曼滤波） # ══════════════════════════════════════════════════════════════════════ class NavigationFilter: """ 简化导航滤波器。 功能： 融合 GPS 观测值和 INS 推算值，输出导航解算状态（nav_state）。 该状态用于 FMS 的航线跟踪决策。 算法： 采用简化的互补滤波（Complementary Filter）， 是卡尔曼滤波的简化版本： nav_pos = α × gps_pos + (1-α) × ins_pos 其中 α = gps_ins_blend（GPS 权重）。 GPS 丢失时 α=0，完全依赖 INS 推算（误差随时间积累）。 与完整卡尔曼滤波的区别： 完整卡尔曼滤波会动态调整增益矩阵， 此处简化为固定权重，足以模拟导航误差对轨迹的影响。 """ def __init__(self, init_state: AircraftState, sensor_params: SensorNoiseParams, seed: Optional[int] = None): """ Args: init_state: 初始真实状态（用于初始化导航解算状态） sensor_params: 传感器噪声参数 seed: 随机种子（用于可重复仿真） """ self.params = sensor_params self._rng = random.Random(seed) # ── INS 积分状态（独立于真实状态）──────────────────────────── # INS 从初始位置开始积分，误差随时间累积 self._ins_lat: float = init_state.latitude self._ins_lon: float = init_state.longitude self._ins_alt: float = init_state.altitude self._ins_heading: float = init_state.heading self._ins_speed: float = init_state.speed # INS 累积漂移量（东向、北向，单位 m） # 用于模拟 INS 位置误差的随机游走特性 self._ins_drift_east: float = 0.0 self._ins_drift_north: float = 0.0 # GPS 丢失状态 self._gps_available: bool = True # 导航解算状态（初始与真实状态相同） self.nav_state: AircraftState = deepcopy(init_state) # 各传感器观测值（供外部分析） self.obs_gps_lat: float = init_state.latitude self.obs_gps_lon: float = init_state.longitude self.obs_baro_alt: float = init_state.altitude self.obs_airspeed: float = init_state.airspeed self.obs_heading: float = init_state.heading def update(self, true_state: AircraftState, dt: float) -> AircraftState: """ 根据真实状态生成传感器观测值，并融合输出导航解算状态。 执行步骤： 1. 用真实状态模拟各传感器测量（加噪声） 2. 用真实速度/航向推算 INS 状态（加漂移） 3. 融合 GPS 和 INS 得到导航解算位置 4. 融合气压计和 GPS 得到导航解算高度 5. 融合空速管和 INS 得到导航解算速度 6. 返回导航解算状态 Args: true_state: 当前真实物理状态 dt: 仿真步长 (s) Returns: nav_state: 导航解算状态（FMS 实际使用的状态） """ p = self.params # ── 步骤 1：GPS 测量 ────────────────────────────────────────── # 模拟 GPS 信号丢失（以概率 gps_dropout_prob 丢失） # 信号丢失时，GPS 观测值保持上一次的值（不更新） self._gps_available = ( self._rng.random() > p.gps_dropout_prob ) if self._gps_available: # GPS 水平噪声（考虑 HDOP 因子放大） # HDOP > 1 时，水平精度下降 h_sigma = p.gps_horizontal_noise * p.gps_hdop # 将水平位置噪声（米）转换为经纬度偏差（度） # 纬度方向：1° ≈ 111320m（与纬度无关） # 经度方向：1° ≈ 111320 × cos(lat) m（随纬度变化） lat_noise_deg = ( self._rng.gauss(0, h_sigma) / 111320.0 ) cos_lat = math.cos(math.radians(true_state.latitude)) lon_noise_deg = ( self._rng.gauss(0, h_sigma) / (111320.0 * max(cos_lat, 0.001)) ) self.obs_gps_lat = true_state.latitude + lat_noise_deg self.obs_gps_lon = true_state.longitude + lon_noise_deg # GPS 垂直噪声（高度） self.obs_gps_alt = ( true_state.altitude + self._rng.gauss(0, p.gps_vertical_noise) ) # ── 步骤 2：INS 推算（积分更新）───────────────────────────── # INS 用上一步的速度和航向推算当前位置 # 误差来源：速度噪声、航向漂移、积分误差 # INS 速度观测（含噪声） ins_speed_obs = max( MIN_SPEED, true_state.speed + self._rng.gauss(0, p.ins_velocity_noise) ) # INS 航向观测（含漂移） # 航向漂移是累积的（随机游走），每步增加一个小随机量 heading_drift_step = self._rng.gauss(0, p.ins_heading_drift * dt) self._ins_heading = ( true_state.heading + heading_drift_step ) % 360.0 # INS 位置漂移（随机游走） # 每步在东向和北向各增加一个随机漂移量 # 漂移量 = drift_rate × dt × 随机方向 drift_magnitude = p.ins_drift_rate * dt self._ins_drift_east += self._rng.gauss(0, drift_magnitude) self._ins_drift_north += self._rng.gauss(0, drift_magnitude) # INS 推算位置 = 真实位置 + 累积漂移 # （真实位置用于初始化，漂移量叠加在上面） drift_dist = math.sqrt( self._ins_drift_east ** 2 + self._ins_drift_north ** 2 ) drift_bearing = math.degrees( math.atan2(self._ins_drift_east, self._ins_drift_north) ) % 360.0 if drift_dist > 0: self._ins_lat, self._ins_lon = GeoUtils.offset_position( true_state.latitude, true_state.longitude, drift_bearing, drift_dist ) else: self._ins_lat = true_state.latitude self._ins_lon = true_state.longitude # ── 步骤 3：气压高度计测量 ──────────────────────────────────── # 气压高度计含固定偏置（气压设定误差）和随机噪声 self.obs_baro_alt = ( true_state.altitude + p.baro_altitude_bias # 固定偏置 + self._rng.gauss(0, p.baro_altitude_noise) # 随机噪声 ) obs_baro_vs = ( true_state.vertical_speed + self._rng.gauss(0, p.baro_vs_noise) ) # ── 步骤 4：空速管测量 ──────────────────────────────────────── self.obs_airspeed = max( MIN_SPEED, true_state.airspeed + p.airspeed_bias + self._rng.gauss(0, p.airspeed_noise) ) # ── 步骤 5：航向传感器测量 ──────────────────────────────────── self.obs_heading = ( true_state.heading + p.heading_bias + self._rng.gauss(0, p.heading_noise) ) % 360.0 # ── 步骤 6：导航融合（互补滤波）────────────────────────────── # GPS 有效时：融合 GPS 和 INS # GPS 丢失时：纯 INS 推算（误差快速累积） if self._gps_available: alpha = p.gps_ins_blend # GPS 权重（典型值 0.8） else: # GPS 丢失：完全依赖 INS，误差随时间积累 alpha = 0.0 # 融合水平位置 nav_lat = (alpha * self.obs_gps_lat + (1.0 - alpha) * self._ins_lat) nav_lon = (alpha * self.obs_gps_lon + (1.0 - alpha) * self._ins_lon) # 融合高度：GPS 垂直精度较差，气压计更可靠 # 典型民航：高度主要用气压计，GPS 作辅助修正 if self._gps_available: # 气压计权重 0.7，GPS 高度权重 0.3 nav_alt = (0.7 * self.obs_baro_alt + 0.3 * self.obs_gps_alt) else: nav_alt = self.obs_baro_alt # 融合航向：使用磁罗盘/AHRS 观测值 nav_heading = self.obs_heading # 融合空速：使用空速管观测值 nav_airspeed = self.obs_airspeed # 垂直速度：使用气压计差分 nav_vs = obs_baro_vs # ── 步骤 7：更新并返回导航解算状态 ─────────────────────────── self.nav_state = true_state.copy_with(latitude = nav_lat, longitude = nav_lon, altitude = nav_alt, heading = nav_heading, airspeed = nav_airspeed, speed = ins_speed_obs, vertical_speed = nav_vs, wind_direction = true_state.wind_direction, wind_speed = true_state.wind_speed) # self.nav_state.latitude = nav_lat # self.nav_state.longitude = nav_lon # self.nav_state.altitude = nav_alt # self.nav_state.heading = nav_heading # self.nav_state.airspeed = nav_airspeed # self.nav_state.speed = ins_speed_obs # self.nav_state.vertical_speed = nav_vs # self.nav_state.wind_direction = true_state.wind_direction # self.nav_state.wind_speed = true_state.wind_speed # self.nav_state.phase = true_state.phase # self.nav_state.timestamp = true_state.timestamp # self.nav_state.elapsed_time = true_state.elapsed_time # self.nav_state.lifecycle = true_state.lifecycle # self.nav_state = AircraftState( # latitude = nav_lat, # longitude = nav_lon, # altitude = nav_alt, # heading = nav_heading, # airspeed = nav_airspeed, # speed = ins_speed_obs, # vertical_speed = nav_vs, # wind_direction = true_state.wind_direction, # wind_speed = true_state.wind_speed, # phase = true_state.phase, # timestamp = true_state.timestamp, # ) return self.nav_state def reset_ins_drift(self) -> None: """ 重置 INS 漂移（模拟 GPS 重新锁定后的 INS 对准）。 在 GPS 信号恢复后调用，将 INS 漂移归零。 """ self._ins_drift_east = 0.0 self._ins_drift_north = 0.0 @property def gps_available(self) -> bool: return self._gps_available @property def ins_drift_distance(self) -> float: """当前 INS 累积漂移距离 (m)""" return math.sqrt( self._ins_drift_east ** 2 + self._ins_drift_north ** 2 ) # ══════════════════════════════════════════════════════════════════════ # 转弯计算工具（保持不变，略） # ══════════════════════════════════════════════════════════════════════ class TurnCalculator: """转弯几何计算（同前版本）""" @staticmethod def compute_turn_radius(speed_ms, bank_angle_deg): bank_rad = math.radians( max(1.0, min(bank_angle_deg, MAX_BANK_ANGLE_LIMIT)) ) return (speed_ms ** 2) / (GRAVITY * math.tan(bank_rad)) @staticmethod def compute_turn_angle(current_heading, target_heading): delta = (target_heading - current_heading) % 360.0 if delta > 180.0: delta -= 360.0 return delta @staticmethod def compute_turn_lead_distance(speed_ms, turn_radius_m, turn_angle_deg): half = math.radians(abs(turn_angle_deg) / 2.0) if half >= math.radians(89.0): return turn_radius_m * 10.0 return turn_radius_m * math.tan(half) @staticmethod def compute_turn_rate(speed_ms, turn_radius_m): if turn_radius_m <= 0 or speed_ms < MIN_TURN_SPEED: return 0.0 return math.degrees(speed_ms / turn_radius_m)