> 本文所述代码提供了一个完整的二维卡尔曼滤波实现，涵盖了从初始化、状态更新到结果可视化的全过程。适合用于学习卡尔曼滤波的基本原理和实际应用。
代码介绍
初始化
- 清理环境：使用 `clear; clc; close all` 清除变量、命令行和图形窗口。
- 随机种子：设置随机数种子以保证结果可重复 (`rng(0)`).
滤波模型初始化
- 时间序列：设置时间从 1 到 1000。
- 噪声协方差矩阵：定义过程噪声 `Q` 和观测噪声 `R`。
- 状态和观测初始化：初始化真实状态 `X`、未滤波状态 `X_` 和观测值 `Z`。
运动模型
- 状态更新：
- 真实状态 `X` 按照单位矩阵更新。
- 带误差的状态 `X_` 通过加入过程噪声 `w` 更新。
- 观测值 `Z` 通过加入观测噪声 `v` 更新。
卡尔曼滤波迭代
- 预测步骤：
- 预测下一个状态 `Xpre` 和状态协方差 `PP`。
- 计算卡尔曼增益 `Kk`。
- 更新步骤：
- 根据观测值更新滤波后的状态 `X_kf` 和协方差 `P`。
可视化
- 状态量曲线绘制：
- 绘制真实值、滤波值和未滤波值的时序图，展示滤波效果。
- 误差对比：
- 绘制滤波前后的绝对误差曲线，比较不同状态下的误差。
- 累积分布函数 (CDF)：
- 绘制误差的 CDF 图，展示滤波前后及观测值的误差分布。
输出结果
误差最大值输出：输出未滤波和滤波后的各维度误差最大值，便于分析滤波效果。
# 运行结果
二维状态曲线：
绝对误差的曲线：
绝对误差的CDF图像，表示误差的概率累积密度，曲线越靠近左上方表示误差整体越小：
如需帮助，或有导航、定位滤波相关的代码定制需求，请通过下方微信联系作者：
