第四章 传感器融合

import numpy as np
from sensor_msgs.msg import LaserScan

class LaserScanFilter:
    def __init__(self, min_range=0.1, max_range=10.0, angle_min=-np.pi, angle_max=np.pi):
        self.min_range = min_range
        self.max_range = max_range
        self.angle_min = angle_min
        self.angle_max = angle_max

    def filter_scan(self, scan_msg):
        """过滤激光扫描数据"""
        ranges = np.array(scan_msg.ranges)
        angles = np.linspace(
            scan_msg.angle_min,
            scan_msg.angle_max,
            len(ranges)
        )

        # 距离过滤
        valid_mask = (ranges >= self.min_range) & (ranges <= self.max_range)

        # 角度过滤
        valid_mask &= (angles >= self.angle_min) & (angles <= self.angle_max)

        # 去除inf和nan
        valid_mask &= np.isfinite(ranges)

        return ranges[valid_mask], angles[valid_mask]

    def to_cartesian(self, ranges, angles):
        """极坐标转笛卡尔坐标"""
        x = ranges * np.cos(angles)
        y = ranges * np.sin(angles)
        return x, y

    def median_filter(self, ranges, window_size=5):
        """中值滤波去噪"""
        filtered = np.copy(ranges)
        half_window = window_size // 2

        for i in range(half_window, len(ranges) - half_window):
            window = ranges[i - half_window:i + half_window + 1]
            valid_window = window[np.isfinite(window)]
            if len(valid_window) > 0:
                filtered[i] = np.median(valid_window)

        return filtered

# ROS订阅示例
import rospy

def scan_callback(msg):
    filter = LaserScanFilter(min_range=0.1, max_range=5.0)

    # 过滤数据
    ranges, angles = filter.filter_scan(msg)

    # 中值滤波
    ranges = filter.median_filter(ranges)

    # 转换为笛卡尔坐标
    x, y = filter.to_cartesian(ranges, angles)

    # 进一步处理...
    print(f"有效点数: {len(x)}")

rospy.init_node('laser_filter')
rospy.Subscriber('/scan', LaserScan, scan_callback)
rospy.spin()

class ObstacleDetector:
    def __init__(self, cluster_distance=0.2, min_points=3):
        self.cluster_distance = cluster_distance
        self.min_points = min_points

    def cluster_points(self, x, y):
        """聚类检测障碍物"""
        points = np.column_stack([x, y])
        clusters = []
        visited = np.zeros(len(points), dtype=bool)

        for i in range(len(points)):
            if visited[i]:
                continue

            cluster = [i]
            visited[i] = True

            for j in range(i + 1, len(points)):
                if visited[j]:
                    continue

                # 计算距离
                dist = np.linalg.norm(points[i] - points[j])

                if dist < self.cluster_distance:
                    cluster.append(j)
                    visited[j] = True

            if len(cluster) >= self.min_points:
                clusters.append(points[cluster])

        return clusters

    def find_closest_obstacle(self, clusters):
        """找最近障碍物"""
        min_dist = float('inf')
        closest_center = None

        for cluster in clusters:
            center = np.mean(cluster, axis=0)
            dist = np.linalg.norm(center)

            if dist < min_dist:
                min_dist = dist
                closest_center = center

        return closest_center, min_dist

# 使用示例
detector = ObstacleDetector(cluster_distance=0.2, min_points=5)

# 假设已有点云数据
x = np.array([1.0, 1.1, 1.2, 3.0, 3.1, 5.0])
y = np.array([0.0, 0.1, 0.0, 2.0, 2.1, 1.0])

clusters = detector.cluster_points(x, y)
print(f"检测到 {len(clusters)} 个障碍物")

closest_center, dist = detector.find_closest_obstacle(clusters)
print(f"最近障碍物距离: {dist:.2f}m, 位置: {closest_center}")

class ICPScanMatcher:
    """迭代最近点(ICP)扫描匹配"""
    def __init__(self, max_iterations=50, tolerance=0.001):
        self.max_iterations = max_iterations
        self.tolerance = tolerance

    def nearest_neighbor(self, src, dst):
        """找最近邻点"""
        distances = np.linalg.norm(src[:, np.newaxis] - dst, axis=2)
        indices = np.argmin(distances, axis=1)
        return indices

    def fit(self, src, dst):
        """ICP配准"""
        src = np.copy(src)

        for iteration in range(self.max_iterations):
            # 找最近邻
            indices = self.nearest_neighbor(src, dst)
            matched_dst = dst[indices]

            # 计算质心
            src_center = np.mean(src, axis=0)
            dst_center = np.mean(matched_dst, axis=0)

            # 去质心
            src_centered = src - src_center
            dst_centered = matched_dst - dst_center

            # SVD求旋转矩阵
            H = src_centered.T @ dst_centered
            U, _, Vt = np.linalg.svd(H)
            R = Vt.T @ U.T

            # 确保是旋转矩阵(det=1)
            if np.linalg.det(R) < 0:
                Vt[-1, :] *= -1
                R = Vt.T @ U.T

            # 计算平移
            t = dst_center - R @ src_center

            # 更新源点云
            src = (R @ src.T).T + t

            # 检查收敛
            mean_error = np.mean(np.linalg.norm(src - matched_dst, axis=1))
            if mean_error < self.tolerance:
                break

        # 提取角度
        theta = np.arctan2(R[1, 0], R[0, 0])

        return R, t, theta

# 使用示例
icp = ICPScanMatcher()

# 源点云和目标点云
src = np.array([[1, 2], [2, 3], [3, 4]])
dst = np.array([[1.1, 2.1], [2.1, 3.1], [3.1, 4.1]])

R, t, theta = icp.fit(src, dst)
print(f"旋转角度: {np.degrees(theta):.2f}°")
print(f"平移: {t}")

import cv2
import numpy as np

class CameraCalibrator:
    def __init__(self, pattern_size=(9, 6), square_size=0.025):
        """
        pattern_size: 棋盘格内角点数量(列,行)
        square_size: 棋盘格方格尺寸(米)
        """
        self.pattern_size = pattern_size
        self.square_size = square_size

        # 3D点
        self.object_points = []
        # 2D点
        self.image_points = []

        # 生成棋盘格3D坐标
        objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
        objp[:, :2] = np.mgrid[0:pattern_size[0],
                               0:pattern_size[1]].T.reshape(-1, 2)
        objp *= square_size
        self.objp = objp

    def add_image(self, image):
        """添加标定图像"""
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

        # 查找棋盘格角点
        ret, corners = cv2.findChessboardCorners(gray, self.pattern_size, None)

        if ret:
            # 亚像素精细化
            criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
            corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

            self.object_points.append(self.objp)
            self.image_points.append(corners2)

            # 绘制角点
            cv2.drawChessboardCorners(image, self.pattern_size, corners2, ret)
            return True, image
        else:
            return False, image

    def calibrate(self, image_size):
        """执行标定"""
        ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
            self.object_points,
            self.image_points,
            image_size,
            None,
            None
        )

        return {
            'camera_matrix': camera_matrix,
            'dist_coeffs': dist_coeffs,
            'rvecs': rvecs,
            'tvecs': tvecs,
            'reprojection_error': ret
        }

    def undistort(self, image, camera_matrix, dist_coeffs):
        """畸变矫正"""
        h, w = image.shape[:2]
        new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(
            camera_matrix, dist_coeffs, (w, h), 1, (w, h))

        # 去畸变
        undistorted = cv2.undistort(image, camera_matrix, dist_coeffs,
                                    None, new_camera_matrix)

        # 裁剪
        x, y, w, h = roi
        undistorted = undistorted[y:y+h, x:x+w]

        return undistorted

# 使用示例
calibrator = CameraCalibrator(pattern_size=(9, 6), square_size=0.025)

# 读取标定图像
import glob
images = glob.glob('calibration_images/*.jpg')

for fname in images:
    img = cv2.imread(fname)
    ret, marked_img = calibrator.add_image(img)
    if ret:
        print(f"成功处理: {fname}")

# 执行标定
image_size = (img.shape[1], img.shape[0])
calib_result = calibrator.calibrate(image_size)

print("相机内参矩阵:")
print(calib_result['camera_matrix'])
print("\n畸变系数:")
print(calib_result['dist_coeffs'])
print(f"\n重投影误差: {calib_result['reprojection_error']:.4f}")

# 保存标定结果
np.savez('camera_calibration.npz',
         camera_matrix=calib_result['camera_matrix'],
         dist_coeffs=calib_result['dist_coeffs'])

class StereoCalibrator:
    def __init__(self, pattern_size=(9, 6), square_size=0.025):
        self.pattern_size = pattern_size
        self.square_size = square_size

        self.objpoints = []
        self.imgpoints_left = []
        self.imgpoints_right = []

        # 3D点
        objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
        objp[:, :2] = np.mgrid[0:pattern_size[0],
                               0:pattern_size[1]].T.reshape(-1, 2)
        objp *= square_size
        self.objp = objp

    def add_stereo_pair(self, img_left, img_right):
        """添加立体图像对"""
        gray_left = cv2.cvtColor(img_left, cv2.COLOR_BGR2GRAY)
        gray_right = cv2.cvtColor(img_right, cv2.COLOR_BGR2GRAY)

        # 查找角点
        ret_left, corners_left = cv2.findChessboardCorners(gray_left, self.pattern_size)
        ret_right, corners_right = cv2.findChessboardCorners(gray_right, self.pattern_size)

        if ret_left and ret_right:
            self.objpoints.append(self.objp)
            self.imgpoints_left.append(corners_left)
            self.imgpoints_right.append(corners_right)
            return True
        return False

    def calibrate_stereo(self, image_size):
        """立体标定"""
        # 先单独标定两个相机
        ret_left, K_left, dist_left, _, _ = cv2.calibrateCamera(
            self.objpoints, self.imgpoints_left, image_size, None, None)

        ret_right, K_right, dist_right, _, _ = cv2.calibrateCamera(
            self.objpoints, self.imgpoints_right, image_size, None, None)

        # 立体标定
        ret, K_left, dist_left, K_right, dist_right, R, T, E, F = cv2.stereoCalibrate(
            self.objpoints,
            self.imgpoints_left,
            self.imgpoints_right,
            K_left, dist_left,
            K_right, dist_right,
            image_size,
            flags=cv2.CALIB_FIX_INTRINSIC
        )

        return {
            'K_left': K_left,
            'dist_left': dist_left,
            'K_right': K_right,
            'dist_right': dist_right,
            'R': R,  # 旋转矩阵
            'T': T,  # 平移向量
            'E': E,  # 本质矩阵
            'F': F   # 基础矩阵
        }

class FeatureMatcher:
    def __init__(self, method='ORB'):
        """
        method: 'SIFT', 'ORB', 'AKAZE'
        """
        if method == 'SIFT':
            self.detector = cv2.SIFT_create()
        elif method == 'ORB':
            self.detector = cv2.ORB_create(nfeatures=1000)
        elif method == 'AKAZE':
            self.detector = cv2.AKAZE_create()

        # BF匹配器
        if method == 'ORB':
            self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        else:
            self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=True)

    def detect_and_compute(self, image):
        """检测特征点并计算描述子"""
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        keypoints, descriptors = self.detector.detectAndCompute(gray, None)
        return keypoints, descriptors

    def match(self, desc1, desc2, ratio=0.75):
        """特征匹配"""
        matches = self.matcher.match(desc1, desc2)

        # 按距离排序
        matches = sorted(matches, key=lambda x: x.distance)

        # Lowe's ratio test
        good_matches = []
        if len(matches) > 1:
            for i, m in enumerate(matches[:-1]):
                if m.distance < ratio * matches[i + 1].distance:
                    good_matches.append(m)

        return good_matches

    def draw_matches(self, img1, kp1, img2, kp2, matches):
        """绘制匹配结果"""
        return cv2.drawMatches(img1, kp1, img2, kp2, matches, None,
                              flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)

# 使用示例
matcher = FeatureMatcher(method='ORB')

img1 = cv2.imread('image1.jpg')
img2 = cv2.imread('image2.jpg')

# 检测特征
kp1, desc1 = matcher.detect_and_compute(img1)
kp2, desc2 = matcher.detect_and_compute(img2)

print(f"图像1特征点数: {len(kp1)}")
print(f"图像2特征点数: {len(kp2)}")

# 匹配
matches = matcher.match(desc1, desc2)
print(f"匹配点数: {len(matches)}")

# 绘制
result = matcher.draw_matches(img1, kp1, img2, kp2, matches[:50])
cv2.imshow('Matches', result)
cv2.waitKey(0)

import cv2
import numpy as np

class YOLODetector:
    def __init__(self, config_path, weights_path, names_path, conf_threshold=0.5):
        self.net = cv2.dnn.readNetFromDarknet(config_path, weights_path)
        self.net.setPreferableBackend(cv2.dnn.DNN_BACKEND_CUDA)
        self.net.setPreferableTarget(cv2.dnn.DNN_TARGET_CUDA)

        # 读取类别名称
        with open(names_path, 'r') as f:
            self.classes = [line.strip() for line in f.readlines()]

        self.conf_threshold = conf_threshold
        self.nms_threshold = 0.4

        # 获取输出层
        layer_names = self.net.getLayerNames()
        self.output_layers = [layer_names[i - 1] for i in self.net.getUnconnectedOutLayers()]

    def detect(self, image):
        """目标检测"""
        height, width = image.shape[:2]

        # 预处理
        blob = cv2.dnn.blobFromImage(image, 1/255.0, (416, 416),
                                    swapRB=True, crop=False)
        self.net.setInput(blob)

        # 前向传播
        outputs = self.net.forward(self.output_layers)

        # 解析结果
        boxes = []
        confidences = []
        class_ids = []

        for output in outputs:
            for detection in output:
                scores = detection[5:]
                class_id = np.argmax(scores)
                confidence = scores[class_id]

                if confidence > self.conf_threshold:
                    # 边界框坐标
                    center_x = int(detection[0] * width)
                    center_y = int(detection[1] * height)
                    w = int(detection[2] * width)
                    h = int(detection[3] * height)

                    x = int(center_x - w / 2)
                    y = int(center_y - h / 2)

                    boxes.append([x, y, w, h])
                    confidences.append(float(confidence))
                    class_ids.append(class_id)

        # NMS非极大值抑制
        indices = cv2.dnn.NMSBoxes(boxes, confidences,
                                   self.conf_threshold, self.nms_threshold)

        results = []
        if len(indices) > 0:
            for i in indices.flatten():
                results.append({
                    'class_id': class_ids[i],
                    'class_name': self.classes[class_ids[i]],
                    'confidence': confidences[i],
                    'box': boxes[i]
                })

        return results

    def draw_detections(self, image, detections):
        """绘制检测结果"""
        for det in detections:
            x, y, w, h = det['box']
            label = f"{det['class_name']}: {det['confidence']:.2f}"

            # 绘制边界框
            cv2.rectangle(image, (x, y), (x + w, y + h), (0, 255, 0), 2)
            cv2.putText(image, label, (x, y - 10),
                       cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

        return image

# ROS集成示例
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class YOLONode:
    def __init__(self):
        rospy.init_node('yolo_detector')

        self.detector = YOLODetector(
            config_path='yolov4.cfg',
            weights_path='yolov4.weights',
            names_path='coco.names'
        )

        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber('/camera/image_raw', Image, self.image_callback)
        self.image_pub = rospy.Publisher('/yolo/image', Image, queue_size=1)

    def image_callback(self, msg):
        # ROS图像转OpenCV
        cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')

        # 检测
        detections = self.detector.detect(cv_image)

        # 绘制
        result_image = self.detector.draw_detections(cv_image, detections)

        # 发布
        result_msg = self.bridge.cv2_to_imgmsg(result_image, 'bgr8')
        self.image_pub.publish(result_msg)

        # 打印
        for det in detections:
            rospy.loginfo(f"检测到: {det['class_name']} ({det['confidence']:.2f})")

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    node = YOLONode()
    node.run()

class ComplementaryFilter:
    def __init__(self, alpha=0.98):
        """
        alpha: 陀螺仪权重(0-1), 越大越信任陀螺仪
        """
        self.alpha = alpha
        self.roll = 0.0
        self.pitch = 0.0

    def update(self, accel, gyro, dt):
        """
        accel: [ax, ay, az] (m/s^2)
        gyro: [gx, gy, gz] (rad/s)
        dt: 时间间隔(s)
        """
        # 加速度计计算角度
        accel_roll = np.arctan2(accel[1], accel[2])
        accel_pitch = np.arctan2(-accel[0],
                                np.sqrt(accel[1]**2 + accel[2]**2))

        # 陀螺仪积分
        gyro_roll = self.roll + gyro[0] * dt
        gyro_pitch = self.pitch + gyro[1] * dt

        # 互补滤波融合
        self.roll = self.alpha * gyro_roll + (1 - self.alpha) * accel_roll
        self.pitch = self.alpha * gyro_pitch + (1 - self.alpha) * accel_pitch

        return self.roll, self.pitch

# 使用示例
cf = ComplementaryFilter(alpha=0.98)

# 模拟IMU数据
dt = 0.01  # 100Hz
for _ in range(100):
    accel = [0.1, 0.2, 9.8]  # m/s^2
    gyro = [0.01, 0.02, 0.03]  # rad/s

    roll, pitch = cf.update(accel, gyro, dt)
    print(f"Roll: {np.degrees(roll):.2f}°, Pitch: {np.degrees(pitch):.2f}°")

class MadgwickFilter:
    def __init__(self, beta=0.1, sample_freq=100):
        """
        beta: 滤波器增益
        sample_freq: 采样频率(Hz)
        """
        self.beta = beta
        self.sample_period = 1.0 / sample_freq
        self.q = np.array([1.0, 0.0, 0.0, 0.0])  # 四元数[w, x, y, z]

    def update(self, gyro, accel, mag=None):
        """
        gyro: [gx, gy, gz] (rad/s)
        accel: [ax, ay, az]
        mag: [mx, my, mz] (可选)
        """
        q = self.q

        # 归一化加速度计
        accel = accel / np.linalg.norm(accel)

        # 目标函数及雅可比矩阵
        f = np.array([
            2*(q[1]*q[3] - q[0]*q[2]) - accel[0],
            2*(q[0]*q[1] + q[2]*q[3]) - accel[1],
            2*(0.5 - q[1]**2 - q[2]**2) - accel[2]
        ])

        J = np.array([
            [-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
            [2*q[1], 2*q[0], 2*q[3], 2*q[2]],
            [0, -4*q[1], -4*q[2], 0]
        ])

        # 梯度下降
        step = J.T @ f
        step = step / np.linalg.norm(step)

        # 四元数导数
        q_dot = 0.5 * self.quaternion_multiply(q, [0, gyro[0], gyro[1], gyro[2]])

        # 更新四元数
        q = q - self.beta * step * self.sample_period + q_dot * self.sample_period

        # 归一化
        self.q = q / np.linalg.norm(q)

        return self.q

    def quaternion_multiply(self, q1, q2):
        """四元数乘法"""
        w = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
        x = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
        y = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]
        z = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]
        return np.array([w, x, y, z])

    def to_euler(self):
        """四元数转欧拉角"""
        w, x, y, z = self.q

        roll = np.arctan2(2*(w*x + y*z), 1 - 2*(x**2 + y**2))
        pitch = np.arcsin(2*(w*y - z*x))
        yaw = np.arctan2(2*(w*z + x*y), 1 - 2*(y**2 + z**2))

        return roll, pitch, yaw

# 使用示例
madgwick = MadgwickFilter(beta=0.1, sample_freq=100)

gyro = [0.01, 0.02, 0.03]
accel = [0.1, 0.2, 9.8]

q = madgwick.update(gyro, accel)
roll, pitch, yaw = madgwick.to_euler()

print(f"四元数: {q}")
print(f"欧拉角: Roll={np.degrees(roll):.2f}°, "
      f"Pitch={np.degrees(pitch):.2f}°, "
      f"Yaw={np.degrees(yaw):.2f}°")

class KalmanFilter1D:
    def __init__(self, process_variance, measurement_variance, initial_value=0):
        self.q = process_variance      # 过程噪声
        self.r = measurement_variance  # 测量噪声

        self.x = initial_value  # 状态估计
        self.p = 1.0           # 误差协方差

    def predict(self):
        """预测步骤"""
        # 状态预测(假设恒定)
        # self.x = self.x

        # 协方差预测
        self.p = self.p + self.q

    def update(self, measurement):
        """更新步骤"""
        # 卡尔曼增益
        k = self.p / (self.p + self.r)

        # 状态更新
        self.x = self.x + k * (measurement - self.x)

        # 协方差更新
        self.p = (1 - k) * self.p

        return self.x

    def filter(self, measurement):
        """完整滤波步骤"""
        self.predict()
        return self.update(measurement)

# 使用示例:传感器融合
kf = KalmanFilter1D(process_variance=0.01, measurement_variance=0.1)

# 模拟带噪声的测量
true_value = 10.0
measurements = true_value + np.random.normal(0, 1, 100)

filtered_values = []
for z in measurements:
    x_filtered = kf.filter(z)
    filtered_values.append(x_filtered)

print(f"真实值: {true_value}")
print(f"测量均值: {np.mean(measurements):.2f}")
print(f"滤波均值: {np.mean(filtered_values):.2f}")

class ExtendedKalmanFilter:
    """机器人位姿估计的EKF"""
    def __init__(self):
        # 状态: [x, y, theta]
        self.x = np.zeros(3)

        # 协方差矩阵
        self.P = np.eye(3) * 0.1

        # 过程噪声
        self.Q = np.diag([0.1, 0.1, 0.05])**2

        # 测量噪声
        self.R = np.diag([0.5, 0.5])**2

    def predict(self, v, omega, dt):
        """预测步骤(运动模型)"""
        x, y, theta = self.x

        # 状态转移(非线性)
        if abs(omega) < 1e-6:
            # 直线运动
            x_new = x + v * np.cos(theta) * dt
            y_new = y + v * np.sin(theta) * dt
            theta_new = theta
        else:
            # 圆弧运动
            x_new = x + v/omega * (np.sin(theta + omega*dt) - np.sin(theta))
            y_new = y + v/omega * (-np.cos(theta + omega*dt) + np.cos(theta))
            theta_new = theta + omega * dt

        # 雅可比矩阵
        if abs(omega) < 1e-6:
            F = np.array([
                [1, 0, -v*np.sin(theta)*dt],
                [0, 1,  v*np.cos(theta)*dt],
                [0, 0, 1]
            ])
        else:
            F = np.array([
                [1, 0, v/omega*(np.cos(theta+omega*dt) - np.cos(theta))],
                [0, 1, v/omega*(np.sin(theta+omega*dt) - np.sin(theta))],
                [0, 0, 1]
            ])

        # 更新状态
        self.x = np.array([x_new, y_new, theta_new])

        # 更新协方差
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z):
        """更新步骤(观测模型)"""
        # 观测: [x_measured, y_measured]
        # 观测模型: h(x) = [x, y]

        # 观测预测
        h = self.x[:2]

        # 观测雅可比
        H = np.array([
            [1, 0, 0],
            [0, 1, 0]
        ])

        # 新息
        y = z - h

        # 新息协方差
        S = H @ self.P @ H.T + self.R

        # 卡尔曼增益
        K = self.P @ H.T @ np.linalg.inv(S)

        # 状态更新
        self.x = self.x + K @ y

        # 协方差更新
        self.P = (np.eye(3) - K @ H) @ self.P

    def get_state(self):
        return self.x, self.P

# 使用示例
ekf = ExtendedKalmanFilter()

# 机器人运动
v = 1.0  # m/s
omega = 0.1  # rad/s
dt = 0.1

for t in range(100):
    # 预测
    ekf.predict(v, omega, dt)

    # 每10步观测一次
    if t % 10 == 0:
        # 模拟GPS观测(带噪声)
        true_x = ekf.x[0]
        true_y = ekf.x[1]
        z = np.array([true_x, true_y]) + np.random.normal(0, 0.5, 2)

        # 更新
        ekf.update(z)

    state, cov = ekf.get_state()
    print(f"t={t*dt:.1f}s: x={state[0]:.2f}, y={state[1]:.2f}, "
          f"theta={np.degrees(state[2]):.1f}°")

# 简单加权
output = alpha * gyro + (1-alpha) * accel

# 最优估计
# 预测
x = F*x + B*u
P = F*P*F' + Q

# 更新
K = P*H' / (H*P*H' + R)
x = x + K*(z - H*x)
P = (I - K*H)*P

# 静止时测量零偏
gyro_bias = np.mean(gyro_samples, axis=0)

# 使用时减去零偏
gyro_corrected = gyro_raw - gyro_bias

# 用加速度计修正长期漂移
# 用磁力计修正偏航漂移
# 互补滤波/卡尔曼滤波

if is_stationary():
    # 静止时重置积分
    integrated_angle = 0

# GPS速度
# 视觉里程计
# 轮速计