第2章：自动驾驶车辆定位

fitness = Σᵢ d²ᵢ / N

inlier_ratio = N_inliers / N_total

if (ICP失败):
    使用IMU预测位姿
    扩大搜索范围
    if (still失败):
        触发重定位

架构图：
                    ┌─────────────┐
                    │ Uncertainty │
                    │   Head      │
                    └──────↑──────┘
                           │
                    ┌──────────────┐
                    │  Pose Head   │
                    └──────↑───────┘
                           │
                  ┌────────────────┐
                  │  Transformer   │
                  │    Decoder     │
                  └────────↑───────┘
                           │
        ┌──────────────────┼──────────────────┐
        │                  │                  │
   ┌────────┐        ┌────────┐        ┌────────┐
   │Feature │        │Feature │        │Feature │
   │Encoder │        │Encoder │        │Encoder │
   └────↑───┘        └────↑───┘        └────↑───┘
        │                  │                  │
   ┌────────┐        ┌────────┐        ┌────────┐
   │  GNSS  │        │  IMU   │        │  SLAM  │
   └────────┘        └────────┘        └────────┘

# Transformer注意力权重
Q = pose_query
K = [gnss_feat, imu_feat, slam_feat]
weights = softmax(Q @ K.T / √d)

L = L_pose + λ₁·L_uncertainty + λ₂·L_consistency

传统：存储 = K × 场景体积
NeRF：存储 = 常数

当场景 > 100m³时，NeRF优势明显

传统：内存线性增长 O(t)
NeRF：内存有界 O(1)

方法          内存占用    每帧处理时间
点云SLAM      1-10 GB     10-100 ms
体素SLAM      100MB-1GB   20-50 ms  
NeRF-SLAM     10-100 MB   50-200 ms
Instant-NGP   50-200 MB   10-30 ms

# 空间网格划分
grid_size = 10  # 10米网格
observations = defaultdict(list)

# 收集车队观测
for vehicle_pose in fleet_data:
    grid_id = to_grid(vehicle_pose.position)
    ref_pose = get_map_reference(grid_id)
    error = vehicle_pose - ref_pose
    observations[grid_id].append(error)

# 统计检验
for grid_id, errors in observations.items():
    if len(errors) > min_samples:
        # t检验判断均值是否显著偏离0
        mean_error = np.mean(errors, axis=0)
        std_error = np.std(errors, axis=0)
        t_stat = mean_error * √n / std_error

        if abs(t_stat) > t_critical:
            systematic_bias[grid_id] = mean_error

# 误差分解模型
# Total_Error = Systematic_Bias + Random_Noise

# 时空一致性分析
def analyze_error_pattern(observations):
    # 空间相关性：相邻网格的误差相似度
    spatial_correlation = compute_spatial_autocorrelation(observations)

    # 时间稳定性：误差随时间的变化
    temporal_variance = compute_temporal_variance(observations)

    # 判断准则
    if spatial_correlation > 0.7 and temporal_variance < threshold:
        return "SYSTEMATIC"
    else:
        return "RANDOM"

# Kalman滤波分离
class BiasEstimator:
    def __init__(self):
        self.bias = np.zeros(3)  # [x, y, heading]
        self.P = np.eye(3) * 1e-3  # 协方差

    def update(self, measurement, confidence):
        # 加权更新
        K = self.P / (self.P + R/confidence)
        self.bias += K * (measurement - self.bias)
        self.P = (I - K) * self.P

# 高斯过程回归建模空间变化的偏差场
class BiasFieldModel:
    def __init__(self):
        self.gp = GaussianProcessRegressor(
            kernel=RBF(length_scale=50.0)  # 50米相关距离
        )
        self.observations = []

    def incremental_update(self, new_obs):
        # 增量学习
        self.observations.append(new_obs)

        # 局部更新（仅重训练受影响区域）
        affected_region = self.get_affected_region(new_obs)
        local_data = self.filter_observations(affected_region)

        # 稀疏GP避免O(n³)复杂度
        if len(local_data) > max_points:
            local_data = self.sparse_selection(local_data)

        self.gp.fit(local_data.positions, local_data.biases)

    def predict_correction(self, position):
        bias, uncertainty = self.gp.predict(position, return_std=True)
        return bias, uncertainty

车辆定位 → 查询修正模型 → 应用修正 → 上传观测
    ↑                                      ↓
    └──────── 云端模型更新 ←───────────────┘

# 异常检测
def validate_observation(obs):
    # Mahalanobis距离检测离群点
    d = mahalanobis(obs, mean, cov)
    if d > chi2_threshold:
        return False

    # 与其他车辆交叉验证
    nearby_obs = get_nearby_observations(obs)
    consensus = compute_consensus(obs, nearby_obs)

    return consensus > min_consensus

# 修正模型验证
def validate_model_update(old_model, new_model):
    # 在保留测试集上评估
    old_error = evaluate(old_model, test_set)
    new_error = evaluate(new_model, test_set)

    # 确保性能提升
    if new_error > old_error * 0.95:  # 至少5%提升
        return False

    # 检查修正量合理性
    max_correction = new_model.get_max_correction()
    if max_correction > 10.0:  # 10米上限
        return False

    return True

         ┌──────────────────────────────┐
         │    融合定位管理器            │
         │  (Fusion Localization Manager)│
         └─────────┬──────────────────┘
                   │
    ┌──────────────┼──────────────┐
    ↓              ↓              ↓
┌─────────┐  ┌─────────┐  ┌─────────┐
│一致性   │  │置信度   │  │状态机   │
│检查器   │  │评估器   │  │控制器   │
└─────────┘  └─────────┘  └─────────┘
     ↑            ↑            ↑
     └────────────┼────────────┘
                  │
   ┌──────┬───────┼───────┬──────┐
   ↓      ↓       ↓       ↓      ↓
┌─────┐┌──────┐┌──────┐┌──────┐┌─────┐
│RTK  ││激光  ││视觉  ││地图  ││IMU  │
│GNSS ││SLAM  ││SLAM  ││匹配  ││预测 │
└─────┘└──────┘└──────┘└──────┘└─────┘

class AnomalyDetector:
    def __init__(self):
        self.history = deque(maxlen=100)
        self.thresholds = {
            'position_jump': 5.0,  # 米
            'velocity_change': 10.0,  # m/s
            'innovation': 3.0,  # 标准差
        }

    def check_consistency(self, measurements):
        """多源一致性检查"""
        results = {}

        # 两两比较
        for i, (name1, pose1) in enumerate(measurements.items()):
            for name2, pose2 in list(measurements.items())[i+1:]:
                distance = np.linalg.norm(pose1.pos - pose2.pos)
                angle_diff = angle_difference(pose1.yaw, pose2.yaw)

                consistency_score = exp(-distance/σ_pos - angle_diff/σ_angle)
                results[(name1, name2)] = consistency_score

        # 计算每个源的平均一致性
        source_scores = defaultdict(list)
        for (s1, s2), score in results.items():
            source_scores[s1].append(score)
            source_scores[s2].append(score)

        return {s: np.mean(scores) for s, scores in source_scores.items()}

    def detect_jump(self, current, previous):
        """检测位置跳变"""
        if previous is None:
            return False

        dt = current.timestamp - previous.timestamp
        distance = np.linalg.norm(current.pos - previous.pos)
        max_distance = self.vehicle_model.max_speed * dt + 3 * σ_gps

        return distance > max_distance

    def innovation_test(self, measurement, prediction, R):
        """新息检验（Innovation Test）"""
        innovation = measurement - prediction
        S = H @ P @ H.T + R  # 新息协方差

        # 马氏距离
        d = innovation.T @ np.linalg.inv(S) @ innovation

        # 卡方检验
        dof = len(innovation)
        threshold = chi2.ppf(0.95, dof)

        return d < threshold

class ConfidenceEstimator:
    def __init__(self):
        self.weights = {
            'rtk': 1.0,
            'lidar_slam': 0.8,
            'visual_slam': 0.6,
            'map_matching': 0.7
        }

    def compute_confidence(self, source_name, measurement):
        base_weight = self.weights[source_name]

        # 动态因子
        factors = []

        if source_name == 'rtk':
            # RTK质量指标
            factors.append(self.rtk_quality_factor(measurement))
            # 卫星数量
            factors.append(min(measurement.num_sats / 10, 1.0))
            # DOP值
            factors.append(1.0 / (1.0 + measurement.hdop))

        elif source_name == 'lidar_slam':
            # 点云配准质量
            factors.append(measurement.fitness_score)
            # 特征丰富度
            factors.append(measurement.feature_count / 1000)
            # 运动一致性
            factors.append(self.motion_consistency(measurement))

        elif source_name == 'visual_slam':
            # 特征匹配数量
            factors.append(min(measurement.num_matches / 500, 1.0))
            # 重投影误差
            factors.append(1.0 / (1.0 + measurement.reprojection_error))
            # 光照条件
            factors.append(self.lighting_quality(measurement))

        elif source_name == 'map_matching':
            # 地图特征匹配度
            factors.append(measurement.matching_score)
            # 地图更新时间
            age_days = (time.now() - measurement.map_timestamp).days
            factors.append(exp(-age_days / 30))  # 30天半衰期

        # 综合置信度
        confidence = base_weight * np.prod(factors)

        # 历史平滑
        confidence = 0.7 * confidence + 0.3 * self.prev_confidence.get(source_name, confidence)
        self.prev_confidence[source_name] = confidence

        return confidence

class LocalizationStateMachine:
    def __init__(self):
        self.states = {
            'NORMAL': self.normal_operation,
            'DEGRADED': self.degraded_operation,
            'MINIMAL': self.minimal_operation,
            'EMERGENCY': self.emergency_stop
        }
        self.current_state = 'NORMAL'
        self.state_history = []

    def update(self, sensor_status):
        # 状态转换逻辑
        transitions = {
            'NORMAL': self.check_normal_transitions,
            'DEGRADED': self.check_degraded_transitions,
            'MINIMAL': self.check_minimal_transitions,
            'EMERGENCY': self.check_emergency_transitions
        }

        new_state = transitions[self.current_state](sensor_status)

        if new_state != self.current_state:
            self.transition_to(new_state)

    def check_normal_transitions(self, status):
        """正常模式转换检查"""
        available = sum(status.values())

        if available >= 3 and status.get('rtk', 0) > 0.8:
            return 'NORMAL'
        elif available >= 2:
            return 'DEGRADED'
        elif available >= 1:
            return 'MINIMAL'
        else:
            return 'EMERGENCY'

    def normal_operation(self):
        """正常运行：全功能"""
        return {
            'max_speed': 120,  # km/h
            'enabled_features': ['lane_change', 'auto_park', 'highway_pilot'],
            'fusion_strategy': 'optimal_weighted'
        }

    def degraded_operation(self):
        """降级运行：限制功能"""
        return {
            'max_speed': 60,
            'enabled_features': ['lane_keep', 'acc'],
            'fusion_strategy': 'conservative'
        }

    def minimal_operation(self):
        """最小运行：仅基础功能"""
        return {
            'max_speed': 30,
            'enabled_features': ['emergency_brake'],
            'fusion_strategy': 'single_best_source'
        }

    def emergency_stop(self):
        """紧急停车"""
        return {
            'max_speed': 0,
            'enabled_features': [],
            'fusion_strategy': 'none',
            'action': 'pull_over_and_stop'
        }

class AdaptiveFusion:
    def __init__(self):
        self.ekf = ExtendedKalmanFilter()

    def fuse(self, measurements, confidences):
        # 根据置信度选择融合策略

        if self.state == 'optimal_weighted':
            # 加权最小二乘
            weights = np.array(list(confidences.values()))
            weights = weights / weights.sum()

            fused_pose = sum(w * m.pose for w, m in 
                           zip(weights, measurements.values()))

            # 不确定性传播
            cov = sum(w**2 * m.cov for w, m in 
                     zip(weights, measurements.values()))

        elif self.state == 'conservative':
            # 保守融合：取不确定性最小的
            best_source = min(measurements.items(), 
                            key=lambda x: np.trace(x[1].cov))
            fused_pose = best_source[1].pose
            cov = best_source[1].cov * 1.5  # 增加不确定性

        elif self.state == 'single_best_source':
            # 单源：选择置信度最高的
            best_source = max(confidences.items(), key=lambda x: x[1])
            fused_pose = measurements[best_source[0]].pose
            cov = measurements[best_source[0]].cov * 2.0

        return fused_pose, cov

# 场景：进入隧道，GNSS信号丢失
def handle_tunnel_scenario():
    # T=0: 检测到GNSS信号减弱
    if gnss.signal_strength < threshold:
        # 预测即将失去GNSS
        prepare_for_gnss_loss()

    # T=1: GNSS完全丢失
    if gnss.is_lost():
        # 切换到SLAM + IMU
        state = 'DEGRADED'
        primary_source = 'lidar_slam'
        secondary_source = 'imu_prediction'

        # 使用最后的GNSS位置作为先验
        slam.set_prior(last_gnss_pose)

    # T=2: SLAM也失效（特征稀疏）
    if slam.fitness < threshold:
        state = 'MINIMAL'
        # 纯IMU + 地图约束
        pose = imu.dead_reckoning()
        pose = map.constrain_to_road(pose)

        # 减速并警告
        vehicle.reduce_speed(30)
        vehicle.warn_driver("定位降级，请准备接管")

    # T=3: 出隧道，逐步恢复
    if gnss.signal_recovered():
        # 渐进式融合，避免跳变
        alpha = 0.1  # 融合系数
        pose = alpha * gnss.pose + (1-alpha) * current_pose

        # 逐步增加GNSS权重
        schedule_weight_increase()