python-kalman: 卡尔曼预测修正算法的 Python 实现

本项目使用 Python 实现卡尔曼预测修正算法（Kalman Filter），针对一维变速直线运动场景，融合位移、速度、加速度测量数据，实时估计真实状态，并在线补偿加速度零偏（系统误差）。代码包含完整的模拟数据生成、滤波迭代、结果可视化与误差统计分析，并配有详细的 LaTeX 技术文档，从基础数理推导到仿真验证，适合学习与工程参考。

---

✨ 特性

• 状态增广：将加速度零偏（系统偏差）纳入状态向量，在线估计并补偿，提高测量精度。
• 灵活的运动模型：支持两种预设运动模式——匀加速和震荡匀加速，可自定义噪声参数和零偏幅值。
• 完整滤波流程：严格遵循预测（先验）、卡尔曼增益计算、更新（后验）五步递推，代码结构清晰。
• 自动数据输出：滤波结果保存为 CSV 文件，同时绘制位移、速度、加速度及零偏估计曲线，便于分析。
• 误差统计：自动计算滤波前后各状态量与真实值的标准差，量化降噪效果。
• 过程噪声建模：采用三阶白噪声模型（加加速度随机游走），配置功率谱密度系数以适应不同动态场景。
• 中文化命名：变量、函数均使用中文命名，降低理解门槛（同时兼容英文环境）。

---

📂 文件结构

python-kalman/
├── 程序/
│   ├── main.py              # 主程序：生成模拟数据、执行滤波、绘图与输出
│   └── 卡尔曼滤波.py        # 卡尔曼滤波器类（通用实现，支持有/无输入）
├── 简介/
│   ├── 卡尔曼预测修正算法简介.tex   # 完整的 LaTeX 技术文档（含推导、建模、仿真）
│   └── fig/                 # 文档引用图片（仿真结果图）
├── output/                  # 运行后自动生成，存放 data.csv 和 fig.png
├── .gitignore
├── LICENSE                  # MIT 许可证
└── README.md               # 本文件

---

🔧 依赖与环境

• Python ≥ 3.10
• NumPy
• Pandas
• Matplotlib

安装依赖（推荐使用虚拟环境）：

pip install numpy pandas matplotlib

---

🚀 快速开始

1. 克隆仓库

git clone https://github.com/your-username/python-kalman.git
cd python-kalman

2. 运行主程序

直接执行 main.py 即可生成模拟数据并进行滤波：

python 程序/main.py

程序会：

• 生成 16384 个采样点（16 秒，采样率 1.024 kHz）；
• 使用预设参数（位移、速度、加速度测量噪声标准差，加速度零偏幅值等）；
• 运行卡尔曼滤波；
• 在 output/ 目录下生成：
  • data.csv：滤波前后各状态量的采样数据（仅保存部分掩膜点，便于查看）；
  • fig.png：四个子图（位移、速度、加速度、零偏估计对比）；
• 在控制台打印滤波前后误差标准差对比表。

3. 自定义参数

您可以在 main.py 的 if __name__ == "__main__": 部分修改：

• 运动模式：生成数据类型 = "匀加速" 或 "震荡匀加速"
• 采样点数 测量数、总时长 总时长
• 测量噪声标准差 位移测量噪声标准差、速度测量噪声标准差、加速度测量噪声标准差
• 加速度系统误差幅值 加速度系统误差阈值
• 过程噪声系数 三阶项系数（三阶白噪声功率谱密度 $\Phi$）

调整后重新运行即可。

---

📊 结果示例

输出图表（output/fig.png）

包含四个子图：

• 位移 ($m$)：真实值、测量值、滤波后曲线，滤波结果几乎与真实值重合。
• 速度 ($m/s$)：展示滤波对速度噪声的抑制效果。
• 加速度 ($m/s^2$)：原始测量受噪声和零偏影响波动较大，滤波后恢复真实趋势。
• 加速度零偏估计：估计的零偏与真实零偏（正弦变化）的对比，验证在线补偿能力。

误差统计（控制台输出）

例如震荡匀加速运动下的输出：

滤波前后标准差对比（越小越好）：
         位移        速度        加速度
滤波前  0.062516  0.249784  212.125381
滤波后  0.003610  0.597679   46.398827

位移和加速度标准差显著降低，速度估计在剧烈动态下精度略有下降但仍能跟踪趋势。

详细仿真分析

请参阅 简介/卡尔曼预测修正算法简介.tex（编译为 PDF），其中包含完整的数学推导、系统建模、两组仿真实验（匀加速和震荡匀加速）的详细讨论以及误差分析。

---

📖 数学背景与设计思路

本项目基于卡尔曼滤波的核心五步递推公式（预测 → 增益 → 更新），并针对一维运动进行了状态扩增。状态向量为 $\mathbf{x} = [l,\ v,\ a,\ a_{\mathrm{bias}}]^\mathrm{T}$，测量向量为 $\mathbf{z} = [l,\ v,\ a_{\mathrm{raw}}]^\mathrm{T}$。系统传递矩阵和测量矩阵分别为：

$$ \tilde{\mathbf{A}} = \begin{bmatrix} 1 & \Delta t & \frac{1}{2}\Delta t^2 & 0 \\ 0 & 1 & \Delta t & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad \tilde{\mathbf{H}} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 1 \end{bmatrix} $$

过程噪声协方差采用三阶白噪声模型，测量噪声协方差取对角阵。代码实现中，对初始时刻采用伪逆法直接由测量初始化状态，从第二步起执行标准卡尔曼迭代。

详细推导过程、协方差矩阵的具体形式以及仿真结果分析，请参见 LaTeX 文档。

---

📝 许可证

本项目采用 MIT 许可证，允许自由使用、修改和分发。

---

🤝 贡献

欢迎提出 Issue 或 Pull Request。如有疑问，可通过作者邮箱联系（见 LICENSE 中的版权信息）。

---

📚 参考文献

• Kalman, R. E. (1960). A New Approach to Linear Filtering and Prediction Problems. Journal of Basic Engineering, 82(1), 35–45.
• 本项目 LaTeX 文档中自含完整推导，可作为学习参考。

---

提示：本项目旨在教学与演示，实际工程应用时请根据具体传感器特性调整噪声协方差参数，并考虑数据融合与非线性扩展（如 EKF/UKF）。