327 lines
14 KiB
Python
327 lines
14 KiB
Python
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)
|