Kalman Filters ⚡ 现代C++17 滤波库

📖 概述

Kalman Filters 是基于现代C++17和Eigen3的通用滤波库。设计上强调可扩展性、编译期优化与工业级鲁棒性。所有滤波器均为模板类，通过 KalmanConfig 定义状态/测量维度，支持静态多态与运行时策略。核心组件包括：

✨ 核心特性

• 模板化配置 — 编译期确定维度，避免运行时开销。
• 多种滤波算法 — LKF, EKF, UKF, 自适应UKF, 精确离散化LKF。
• 高级离散化 — 矩阵指数离散化(精确)、改进欧拉(二阶精度)。
• 自适应噪声估计 — 基于滑动窗口的Q/R自适应调整。
• 多传感器融合 — 支持异步传感器、可靠性评分、联邦融合。
• 性能监控 — NEES, 创新序列, 滤波器一致性检验。
• 示例丰富 — 目标跟踪、机器人定位、圆周运动、弹簧阻尼系统。
• CMake集成 — 支持安装、导出、测试。

⚡ 快速开始

最简单的线性卡尔曼滤波示例（2D跟踪）

#include "kalman_filters/linear_kalman_filter.hpp"

struct MyConfig : KalmanConfig<4, 2> {};  // 状态4维，测量2维

int main() {
    MyConfig::StateMatrix F; F << 0,0,1,0, 0,0,0,1, 0,0,0,0, 0,0,0,0;
    MyConfig::MeasurementMatrix H; H << 1,0,0,0, 0,1,0,0;
    MyConfig::StateMatrix Q = MyConfig::StateMatrix::Identity() * 0.1;
    Eigen::Matrix2d R = Eigen::Matrix2d::Identity() * 1.0;

    LinearKalmanFilter<MyConfig> kf(F, H, Q, R);
    MyConfig::StateVector x0; x0 << 0,0,1,1;
    kf.init(x0, MyConfig::StateMatrix::Identity() * 10);

    // 预测 + 更新
    kf.predict(1.0);
    MyConfig::MeasurementVector z; z << 1.1, 0.9;
    kf.update(z);
    auto state = kf.getState();
    return 0;
}

📦 编译与依赖

依赖: Eigen3 (≥3.3), CMake (≥3.12), C++17编译器

# Ubuntu 安装依赖
sudo apt install libeigen3-dev cmake build-essential

# 构建
git clone https://github.com/example/kalman_filters.git
cd kalman_filters
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON
make -j4
# 运行示例
./linear_example
./optimized_ukf_example

🏗️ 架构设计

所有滤波器继承自 KalmanFilterBase<Config>，统一 predict/update 接口。关键抽象：

• KalmanConfig<StateDim, MeasDim, ControlDim> — 定义类型别名。
• StateTransitionFunction / MeasurementFunction — 支持函数式编程。
• discretization 命名空间 — 矩阵指数、改进欧拉、零阶保持。
• 性能追踪: PerformanceMetrics 包含NEES、创新标准差。

📈 线性卡尔曼滤波 (LinearKalmanFilter)

适用于线性系统 x_{k+1}=F·x_k + B·u + w，z_k=H·x_k + v。内部使用欧拉离散化(但可通过精确离散化类提升)。

• predict(dt, u) — 状态和协方差外推。
• update(z) — Joseph形式协方差更新，数值稳定。
• 支持控制矩阵 setControlMatrix()。

🌀 扩展卡尔曼滤波 (ExtendedKalmanFilter)

对非线性系统进行一阶泰勒展开，使用数值雅可比（中心差分）。适用于中等非线性系统。参数 epsilon 控制微分步长。

示例场景：雷达极坐标测量（距离+角度）与笛卡尔状态之间的变换。

auto h = [](const StateVector& x) -> MeasurementVector {
    double r = std::sqrt(x(0)*x(0)+x(1)*x(1));
    double theta = std::atan2(x(1), x(0));
    return {r, theta};
};
ExtendedKalmanFilter<Config> ekf(f, h, Q, R);

🎯 无迹卡尔曼滤波 (UnscentedKalmanFilter)

通过确定性Sigma点传递非线性，避免雅可比计算，精度高达二阶泰勒。支持参数 alpha, beta, kappa 调节Sigma分布。

• 默认参数: alpha=1e-3, beta=2 (高斯最优), kappa=0 → 自动优化。
• 用于强非线性系统，如恒定转弯率模型、圆周运动。

⚙️ 精确离散化 LKF (PreciseLinearKalmanFilter)

对于连续时间系统 dx/dt = F_cont·x + w，使用矩阵指数 expm 获得精确离散化，或使用改进欧拉(二阶)。适用于高动态系统。

PreciseLinearKalmanFilter<Config> kf(F_cont, H, Q_cont, R, "exact");
kf.predict(dt);   // 内部精确离散化

discretization.hpp 提供三种方法: exact, improved, euler。

🔄 自适应线性卡尔曼滤波 (AdaptiveLinearKalmanFilter)

基于滑动窗口创新序列在线估计测量噪声协方差R，动态调整滤波增益，适应环境变化。通过 adaptive_factor 衡量。

使用Joseph形式协方差更新增强数值稳定性。

🚀 优化UKF + 自适应噪声 (OptimizedUnscentedKalmanFilter)

继承自标准UKF，增加:

• 在线Q/R自适应（指数加权移动平均 + Huber鲁棒估计）。
• 参数动态优化（根据非线性程度调整alpha/kappa）。
• 性能指标收集: NEES, 创新序列, 滤波器一致性。
• 可实时获取 getPerformanceMetrics()。

🔗 多传感器融合 (MultiSensorFusionFilter)

继承优化UKF，支持注册多种传感器（GPS/Lidar等），每个传感器有自己的测量函数、噪声协方差和动态可靠性评分。融合采用联邦滤波框架，通过马氏距离更新可靠性。

fusion_filter.registerSensor(0, "GPS", gps_h, gps_cov, 0.9);
fusion_filter.registerSensor(1, "Lidar", lidar_h, lidar_cov, 0.8);
fusion_filter.fusionUpdate(measurements);   // 批量更新

multi_sensor_fusion_example.cpp 展示融合GPS与激光雷达，并保存性能CSV。

📌 示例程序一览

⚙️ 配置与类型系统

KalmanConfig 是核心配置模板，定义状态/测量维度以及对应的Eigen类型。

template<int StateDim, int MeasurementDim, int ControlDim = 0>
struct KalmanConfig {
    static constexpr int state_dim = StateDim;
    static constexpr int measurement_dim = MeasurementDim;
    using StateVector = Eigen::Matrix<double, StateDim, 1>;
    using StateMatrix = Eigen::Matrix<double, StateDim, StateDim>;
    // ...
};

所有滤波器均使用该配置，保证类型安全与编译期优化。

📊 性能指标与调试

OptimizedUnscentedKalmanFilter 提供 PerformanceMetrics 结构:

• mean_innovation — 平均新息范数，反映预测偏差。
• innovation_std — 新息标准差。
• nees — 归一化估计误差平方，理想值接近状态维度。
• filter_consistency — nees/state_dim，接近1为一致。

通过 visualization::PerformanceLogger 记录时间序列并导出CSV，方便离线分析。

⚖️ 许可证