卡曼滤波器调参

如果卡尔曼滤波器的表现不佳，你需要调整的主要是对系统中“不确定性”进行建模的三个关键参数：

self.matrix_R (测量噪声协方差):
- 这是最重要的“旋钮”。它告诉滤波器你的传感器有多可靠。
- 预测过于滞后/迟钝？ -> R 可能太大了，减小 R，让滤波器更相信新的测量值。
- 预测过于跳动/不稳定？ -> R 可能太小了，增大 R，让滤波器更多地依赖平滑的物理模型。
self.matrix_Q (过程噪声协方差):
- 它告诉滤波器你的物理模型有多可靠。
- 滤波器跟不上小行星的加速或转弯？ -> Q 可能太小了，增大 Q，让滤波器能更快地调整对速度和加速度的估计。
- 预测一直很发散，不稳定？ -> Q 可能太大了，减小 Q，让滤波器更相信物理模型。
初始 matrix_P (首次观测时设置):
- 它代表了你对新出现小行星初始状态的巨大不确定性。
- 初始收敛太慢？ -> 增大初始 P 的值（例如从 1000 到 10000）。这会让滤波器在开始时更倚重测量值，从而更快地估计出初始速度。

调试策略: 先从 R 开始调，找到一个视觉上看起来合理的平衡点；然后调整 Q 来优化对运动变化的跟踪能力；最后，如果初始几步有问题，再调整初始 P。

卡曼滤波器的公式

卡尔曼滤波器是一个迭代过程，分为两个主要阶段：预测（Predict） 和 更新（Update）。

此阶段利用前一时刻的估计值来预测当前时刻的状态。

状态预测方程 (Predicted State Estimate):
- $\hat{x}_{k|k-1}$ : 当前时刻的预测状态（基于 k-1 时刻的估计）
- $F_k$ : 状态转移矩阵，描述了系统状态如何从 k-1 时刻演变到 k 时刻。
- $\hat{x}_{k-1|k-1}$ : k-1 时刻的最优估计状态。
- $B_k$ : 控制输入矩阵。
- $u_k$ : 控制向量（如果没有控制输入，此项为0）。
协方差预测方程 (Predicted Covariance Estimate):
- $P_{k|k-1}$ : 当前时刻的预测协方差（衡量预测状态的不确定性）。
- $P_{k-1|k-1}$ : k-1 时刻的最优估计协方差。
- $F_k^T$ : 状态转移矩阵的转置。
- $Q_k$ : 过程噪声协方差矩阵，描述了模型本身的不可预测性或系统固有的随机扰动。

此阶段结合了新的测量值来修正预测阶段的结果，得到当前时刻的最优估计。

卡尔曼增益 (Kalman Gain):
- $K_k$ : 卡尔曼增益，决定了在融合预测和测量时，各占多少比重。它在 0 和 1 之间，越大表示越信任测量值。
- $H_k$ : 观测矩阵，将状态空间映射到测量空间。
- $R_k$ : 测量噪声协方差矩阵，描述了传感器测量值的不确定性。
状态更新方程 (Updated State Estimate):
- $\hat{x}_{k|k}$ : 当前时刻的最优估计状态（融合了测量值）。
- $z_k$ : 当前时刻的实际测量值。
- $(z_k - H_k \hat{x}_{k|k-1})$ : 测量残差，即实际测量值与预测测量值之间的差异。
协方差更新方程 (Updated Covariance Estimate):
- $P_{k|k}$ : 当前时刻的最优估计协方差。
- $I$ : 单位矩阵。

这些公式详细说明了卡尔曼滤波器如何通过预测和更新的循环，从带有噪声的测量中估计出系统状态。

✅ 关键点摘要:

💡 后续思考:
如果你想深入了解这些矩阵（F, H, Q, R）的具体构建方式，或者想看一个具体的例子（例如，一个物体在二维空间中的运动），请告诉我！