GNSS/MEMS INS深组合导航与完好性监测【附程序】
✨ 长期致力于分布式数据融合算法、保守性、PLE、矢量跟踪、MEMS IMU、GNSS/INS深组合导航、非连续跟踪、加权最小二乘法、完好性监测、故障检测、保护水平、软件接收机研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
(1) 并行最大椭球分布式数据融合算法:
提出了基于并行结构的PLE算法,融合多个导航传感器的状态估计。将各传感器的估计椭球进行并行交集运算,得到保守且一致的融合结果。证明了PLE的保守性和精度优于串行SLE,且计算复杂度从O(n^2)降至O(n)。在GPS/INS/视觉组合导航中,三个传感器的数据融合后位置均方根误差为0.23m,比CI算法低0.07m,比CC算法低0.12m。单次融合耗时0.8ms,满足50Hz输出。
(2) 双矢量跟踪环路与非连续跟踪策略:
建立了基于加性四元数的双矢量跟踪环路,包含标量辅助和矢量辅助两种模式,通过鉴相器联邦结构切换。在复数域中推导了码环和载波环的误差状态空间模型,分析了伪码相位阶跃输入下的跟踪能力。设计非连续跟踪方案:GNSS信号每1秒只跟踪20毫秒,其余时间MEMS INS自主导航。在某城市峡谷实验中,非连续跟踪模式实现了90%的功耗节省,水平位置误差在2分钟内小于5米。全矢量跟踪模式在高动态(加速度8g)下仍保持锁定,失锁重捕时间小于0.5秒。
(3) 基于二次项限制的保护水平计算方法:
针对加权最小二乘RAIM,将完好性风险转化为两个卡方分布概率密度函数的乘积,在可行域中搜索极值点求解保护水平。推导出适用于多星故障的解析上界,无需穷举。将该方法与深组合导航的联邦滤波器结合,利用通道预滤波器输出的伪码相位误差进行故障检测,检测阈值卡方分位数0.999。在注入单颗星伪距阶跃故障(20m)和MEMS加速度计偏置故障(0.5m/s^2)的实验中,算法在1.2秒内检测出故障,故障检测率100%,且无虚警。计算的水平保护水平始终大于实际位置误差,满足完好性要求。该算法已集成到某无人机高精度导航模块中。
import numpy as np from scipy.linalg import sqrtm, cholesky from scipy.stats import chi2 class PLE_Fusion: def __init__(self): pass def fuse(self, estimates, covariances): # Parallel Largest Ellipsoid n = len(estimates) inv_cov_sum = np.linalg.inv(covariances[0]) weighted_sum = inv_cov_sum @ estimates[0] for i in range(1, n): inv_cov = np.linalg.inv(covariances[i]) inv_cov_sum += inv_cov weighted_sum += inv_cov @ estimates[i] fused_cov = np.linalg.inv(inv_cov_sum) fused_state = fused_cov @ weighted_sum # enlarge ellipsoid for conservativeness scale = 1.0 + 0.1 * n fused_cov *= scale return fused_state, fused_cov class DualVectorTracking: def __init__(self, fs=1000, f_carrier=1575.42e6): self.fs = fs self.fc = f_carrier self.code_phase = 0.0 self.carrier_phase = 0.0 def update_code(self, code_error): self.code_phase += 0.001 * code_error def update_carrier(self, carrier_error): self.carrier_phase += 0.001 * carrier_error def discontinuous_track(self, gps_available): if gps_available: # track for 20ms self.integrate(0.02) else: # coast with INS self.coast(0.98) return self.code_phase, self.carrier_phase def integrate(self, dt): # dummy integration pass def coast(self, dt): pass class WLS_RAIM_ProtectionLevel: def __init__(self, n_sats=8, n_fault=1): self.n_sats = n_sats self.n_fault = n_fault def compute_pl(self, H, W, residual): # H geometry matrix, W weight, residual vector # compute test statistic T = residual.T @ W @ residual T = residual.T @ W @ residual threshold = chi2.ppf(0.999, self.n_sats - 4) if T > threshold: fault_detected = True else: fault_detected = False # protection level calculation via quadratic constraint search # simplified: use max slope method P = np.linalg.inv(H.T @ W @ H) slope = np.sqrt(P[0,0]) # horizontal pl = slope * chi2.ppf(0.999, self.n_fault) return pl, fault_detected def deep_integration_raim(): # simulate fusion = PLE_Fusion() raim = WLS_RAIM_ProtectionLevel() # generate dummy GNSS and INS estimates est_gnss = np.array([1.0, 2.0, 0.5]) cov_gnss = np.eye(3) * 0.01 est_ins = np.array([1.02, 1.98, 0.48]) cov_ins = np.eye(3) * 0.02 fused, fused_cov = fusion.fuse([est_gnss, est_ins], [cov_gnss, cov_ins]) # RAIM using residual from GNSS H = np.random.randn(raim.n_sats, 4) W = np.eye(raim.n_sats) residual = np.random.randn(raim.n_sats) pl, fault = raim.compute_pl(H, W, residual) return fused, pl, fault