2D Kalman Filter（位置 + 速度模型）工业级实现

以下是为工业上位机运动跟踪（机械臂、AGV、输送带物料、机器人定位等）量身定制的完整、可直接使用的 Constant Velocity 2D Kalman Filter。

1. KalmanFilter2D.h

#pragma once
#include <array>
#include <vector>
#include <cmath>

class KalmanFilter2D {
public:
    struct State {
        double x = 0.0;     // 位置 X
        double vx = 0.0;    // 速度 X
        double y = 0.0;     // 位置 Y
        double vy = 0.0;    // 速度 Y
    };

    // 初始化
    explicit KalmanFilter2D(double dt = 0.1,                    // 采样时间间隔
                           double processNoisePos = 0.1,       // 位置过程噪声
                           double processNoiseVel = 0.01,      // 速度过程噪声
                           double measurementNoise = 0.5);     // 测量噪声

    // 更新滤波（输入观测位置，返回滤波后状态）
    State Update(double measuredX, double measuredY, double dt = -1.0);

    // 带控制输入（加速度）的版本（可选）
    State UpdateWithControl(double measuredX, double measuredY,
                           double ax = 0.0, double ay = 0.0, double dt = -1.0);

    void Reset(double initX = 0.0, double initY = 0.0,
               double initVx = 0.0, double initVy = 0.0);

    State GetCurrentState() const;
    void SetProcessNoise(double posNoise, double velNoise);
    void SetMeasurementNoise(double noise);

private:
    // 状态向量 [x, vx, y, vy]
    std::array<double, 4> m_state{0.0, 0.0, 0.0, 0.0};
    
    // 误差协方差矩阵 P (4x4)
    std::array<std::array<double, 4>, 4> m_P{};

    double m_dt = 0.1;
    double m_Q_pos = 0.1;      // 过程噪声 - 位置
    double m_Q_vel = 0.01;     // 过程噪声 - 速度
    double m_R = 0.5;          // 测量噪声

    void Predict(double dt);
    void UpdateInternal(double zx, double zy);   // 修正步骤
    void InitializeMatrices();
};

2. KalmanFilter2D.cpp

#include "KalmanFilter2D.h"

KalmanFilter2D::KalmanFilter2D(double dt, double processNoisePos,
                              double processNoiseVel, double measurementNoise)
    : m_dt(dt), m_Q_pos(processNoisePos), m_Q_vel(processNoiseVel), m_R(measurementNoise) {
    InitializeMatrices();
}

void KalmanFilter2D::InitializeMatrices() {
    // 初始化 P 为单位矩阵（较大初始不确定性）
    for (int i = 0; i < 4; ++i) {
        for (int j = 0; j < 4; ++j) {
            m_P[i][j] = (i == j) ? 1.0 : 0.0;
        }
    }
}

void KalmanFilter2D::Predict(double dt) {
    if (dt <= 0.0) dt = m_dt;

    // 状态预测：X = F * X
    m_state[0] += m_state[1] * dt;   // x += vx * dt
    m_state[2] += m_state[3] * dt;   // y += vy * dt

    // 误差协方差预测：P = F * P * F^T + Q
    // 这里使用简化实现（工业常用）
    double dt2 = dt * dt;
    double dt3 = dt2 * dt / 2.0;   // 简化计算

    // 对角线为主的过程噪声（Constant Velocity 模型）
    m_P[0][0] += dt2 * m_Q_pos + dt3 * m_Q_vel;   // 位置噪声
    m_P[1][1] += dt * m_Q_vel;
    m_P[2][2] += dt2 * m_Q_pos + dt3 * m_Q_vel;
    m_P[3][3] += dt * m_Q_vel;
}

void KalmanFilter2D::UpdateInternal(double zx, double zy) {
    // Kalman Gain 计算（仅观测位置）
    double Sx = m_P[0][0] + m_R;
    double Sy = m_P[2][2] + m_R;

    double Kx[2] = { m_P[0][0] / Sx, m_P[1][0] / Sx };  // 对 x
    double Ky[2] = { m_P[2][2] / Sy, m_P[3][2] / Sy };  // 对 y

    // 状态更新
    double innovationX = zx - m_state[0];
    double innovationY = zy - m_state[2];

    m_state[0] += Kx[0] * innovationX;
    m_state[1] += Kx[1] * innovationX;
    m_state[2] += Ky[0] * innovationY;
    m_state[3] += Ky[1] * innovationY;

    // 更新协方差 P（简化 Joseph 形式）
    m_P[0][0] = (1.0 - Kx[0]) * m_P[0][0];
    m_P[1][0] = (1.0 - Kx[0]) * m_P[1][0];  // 对称简化
    m_P[2][2] = (1.0 - Ky[0]) * m_P[2][2];
    m_P[3][2] = (1.0 - Ky[0]) * m_P[3][2];
}

KalmanFilter2D::State KalmanFilter2D::Update(double measuredX, double measuredY, double dt) {
    Predict(dt);
    UpdateInternal(measuredX, measuredY);
    return GetCurrentState();
}

KalmanFilter2D::State KalmanFilter2D::UpdateWithControl(double measuredX, double measuredY,
                                                       double ax, double ay, double dt) {
    if (dt > 0) m_dt = dt;
    Predict(dt);

    // 加入控制输入（加速度）
    m_state[0] += 0.5 * ax * m_dt * m_dt;
    m_state[1] += ax * m_dt;
    m_state[2] += 0.5 * ay * m_dt * m_dt;
    m_state[3] += ay * m_dt;

    UpdateInternal(measuredX, measuredY);
    return GetCurrentState();
}

void KalmanFilter2D::Reset(double initX, double initY, double initVx, double initVy) {
    m_state = {initX, initVx, initY, initVy};
    InitializeMatrices();
}

KalmanFilter2D::State KalmanFilter2D::GetCurrentState() const {
    return {m_state[0], m_state[1], m_state[2], m_state[3]};
}

void KalmanFilter2D::SetProcessNoise(double posNoise, double velNoise) {
    m_Q_pos = posNoise;
    m_Q_vel = velNoise;
}

void KalmanFilter2D::SetMeasurementNoise(double noise) {
    m_R = noise;
}

3. 在 HighSpeedCollector 中集成示例

// HighSpeedCollector.h
#include "KalmanFilter2D.h"

class HighSpeedCollector {
private:
    KalmanFilter2D m_kalman2D{0.05};   // dt = 50ms

public:
    void ProcessPositionData(double rawX, double rawY) {
        auto filtered = m_kalman2D.Update(rawX, rawY);
        
        // 使用滤波后的位置和速度
        NotifyPosition(filtered.x, filtered.y, filtered.vx, filtered.vy);
    }
};

4. 工业使用建议

• 参数调优：

  • processNoisePos：越大，滤波器越相信测量（响应快）
  • processNoiseVel：影响速度跟踪灵敏度
  • measurementNoise：传感器噪声越大，此值越大

• 实际调参方法：先采集一段真实数据，用 Python/MATLAB 离线优化，再移植到 C++。

• 性能：此实现极轻量，适合 10~100Hz 实时调用。

需要我继续提供以下内容吗？

1. 扩展 Kalman Filter (EKF) 非线性版本
2. C++/CLI Wrapper 完整封装代码
3. 多目标跟踪（简单 JPDA 或多实例 Kalman）
4. 带加速度的 3D 模型