以下是针对 卡尔曼滤波（Kalman Filter）平滑定位 的完整、工业级实战指南，特别适合 AGV、机器人、仓储叉车、无人搬运车等场景中对目标位置（或自身位置）的平滑处理。

卡尔曼滤波在工业视觉/定位中的核心作用是：

• 消除传感器噪声（激光雷达、视觉、IMU、轮速计等数据抖动）
• 预测短暂丢失时的位置（目标被遮挡、传感器盲区）
• 融合多传感器数据（视觉 + IMU + 轮速计）
• 提供平滑、连续、可信的实时位置输出

我们以 AGV 在仓储场景中对前方货架/物料盒的实时追踪平滑 为例，完整实现一个 卡尔曼滤波器，并结合 YOLO 检测结果进行位置平滑。

一、卡尔曼滤波在 AGV 定位中的典型应用场景

1. 目标追踪平滑：YOLO 每帧检测到货架坐标，但由于光照、遮挡、运动模糊，位置会抖动 → 用卡尔曼预测平滑输出。
2. AGV 自身定位融合：轮速计漂移 + IMU 噪声 + 视觉里程计 → 卡尔曼融合多源数据。
3. 预测未来位置：提前 0.5~1s 预测目标位置，提前规划避障路径。

二、数学模型（简明版，工业够用）

我们采用最常见的 匀速直线运动模型（Constant Velocity Model），状态向量为：

状态向量 x = [x, y, vx, vy]ᵀ
（位置 x/y + 速度 vx/vy）

状态转移方程（预测）：

x(k+1) = F · x(k) + w(k)
F = [[1, 0, Δt, 0],
     [0, 1, 0, Δt],
     [0, 0, 1, 0],
     [0, 0, 0, 1]]

观测方程（YOLO 检测到的像素坐标）：

z(k) = H · x(k) + v(k)
H = [[1, 0, 0, 0],
     [0, 1, 0, 0]]   （只观测位置，不直接观测速度）

噪声：

• Q：过程噪声协方差（运动模型不确定性）
• R：测量噪声协方差（YOLO 检测误差，通常 5~20 像素²）

三、C# 完整实现（MathNet.Numerics + OpenCVSharp）

NuGet 安装：

dotnet add package MathNet.Numerics
dotnet add package OpenCvSharp4

核心卡尔曼滤波类（支持像素坐标平滑，也可扩展到世界坐标）

using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using OpenCvSharp;
using System;

public class KalmanPositionTracker
{
    private Matrix<double> F;          // 状态转移矩阵
    private Matrix<double> H;          // 观测矩阵
    private Matrix<double> Q;          // 过程噪声协方差
    private Matrix<double> R;          // 测量噪声协方差
    private Matrix<double> P;          // 估计协方差
    private Vector<double> x;          // 状态估计 [x, y, vx, vy]

    private double dt;                 // 时间间隔（秒）

    public KalmanPositionTracker(double deltaTime = 1.0 / 30.0,   // 默认30fps
                                 double processNoise = 0.01,      // 运动模型噪声
                                 double measurementNoise = 10.0)  // 像素测量噪声（可调）
    {
        dt = deltaTime;

        // 状态转移矩阵 F (匀速模型)
        F = DenseMatrix.OfArray(new double[,]
        {
            {1, 0, dt, 0},
            {0, 1, 0, dt},
            {0, 0, 1, 0},
            {0, 0, 0, 1}
        });

        // 观测矩阵 H (只观测 x,y 位置)
        H = DenseMatrix.OfArray(new double[,]
        {
            {1, 0, 0, 0},
            {0, 1, 0, 0}
        });

        // 过程噪声协方差 Q
        Q = DenseMatrix.CreateIdentity(4) * processNoise;

        // 测量噪声协方差 R
        R = DenseMatrix.CreateIdentity(2) * measurementNoise;

        // 初始协方差 P（初始不确定性较大）
        P = DenseMatrix.CreateIdentity(4) * 1000;

        // 初始状态（位置速度都设为0）
        x = DenseVector.OfArray(new double[] { 0, 0, 0, 0 });
    }

    /// <summary>
    /// 使用新的测量值更新卡尔曼滤波器
    /// </summary>
    /// <param name="measurement">当前帧检测到的中心点 (x, y) 像素坐标</param>
    public void Update(Point2f measurement)
    {
        // 1. 预测
        x = F * x;
        P = F * P * F.Transpose() + Q;

        // 2. 计算卡尔曼增益
        var y = DenseVector.OfArray(new[] { measurement.X, measurement.Y }) - H * x;
        var S = H * P * H.Transpose() + R;
        var K = P * H.Transpose() * S.Inverse();

        // 3. 更新状态估计
        x += K * y;

        // 4. 更新协方差
        P = (DenseMatrix.CreateIdentity(4) - K * H) * P;
    }

    /// <summary>
    /// 预测下一帧位置（不更新状态）
    /// </summary>
    public Point2f PredictNext()
    {
        var predicted = F * x;
        return new Point2f((float)predicted[0], (float)predicted[1]);
    }

    /// <summary>
    /// 获取当前滤波后的平滑位置
    /// </summary>
    public Point2f GetSmoothedPosition()
    {
        return new Point2f((float)x[0], (float)x[1]);
    }

    /// <summary>
    /// 获取当前速度估计
    /// </summary>
    public Point2f GetVelocity()
    {
        return new Point2f((float)x[2], (float)x[3]);
    }
}

四、完整集成示例（YOLO检测 → 卡尔曼平滑 → 输出）

public class AGVTargetTracker
{
    private readonly KalmanPositionTracker _kalman;
    private readonly YoloV9Detector _detector;  // 前面文章中的 YOLOv9 检测器

    public AGVTargetTracker(YoloV9Detector detector)
    {
        _detector = detector;
        _kalman = new KalmanPositionTracker(dt: 1.0 / 30.0, processNoise: 0.005, measurementNoise: 8.0);
    }

    public Point2f TrackTarget(Bitmap frame)
    {
        var detections = _detector.Detect(frame);

        // 假设我们只追踪第一个检测到的目标（可扩展多目标）
        if (detections.Count == 0)
        {
            // 未检测到 → 用卡尔曼预测继续平滑
            return _kalman.PredictNext();
        }

        var target = detections[0];
        var center = new Point2f(
            target.BoundingBox.X + target.BoundingBox.Width / 2,
            target.BoundingBox.Y + target.BoundingBox.Height / 2);

        // 更新卡尔曼滤波
        _kalman.Update(center);

        // 返回平滑后的位置
        return _kalman.GetSmoothedPosition();
    }
}

五、工业场景关键调优经验

六、常见坑与解决方案

1. 初始位置不准：第一次检测时卡尔曼状态为0，导致预测漂移 → 第一次直接用检测值初始化 x。
2. 速度估计发散：过程噪声 Q 太小 → 速度估计会越来越大 → 适当增大 Q。
3. 目标突然加速：匀速模型失效 → 可升级为匀加速模型（状态向量加 ax, ay）。
4. 性能瓶颈：高帧率下卡尔曼计算开销可忽略，但 YOLO推理是瓶颈 → 用 YOLOv9n + TensorRT 加速。

如果您需要以下任一方向的进一步完整代码，请直接回复：

• 完整 WinForms / Avalonia 示例（实时视频 + YOLO框 + 卡尔曼平滑轨迹）
• 多目标卡尔曼追踪（匈牙利匹配 + 目标ID管理）
• 卡尔曼 + IMU 融合（位置 + 加速度）
• 实际项目性能实测（Jetson Orin / 研华工控机 FPS 与延迟）
• 丢失重检测 + 轨迹预测完整逻辑

随时补充！祝您的 AGV 追踪项目跑得稳稳的！