卡尔曼滤波算法（Kalman Filter, KF）深入推导
卡尔曼滤波是一种基于贝叶斯估计和最小均方误差（MMSE）的最优状态估计算法。它适用于线性高斯系统，其核心思想是对状态进行递归估计，在预测和测量之间动态调整，最小化估计误差。
1. 线性系统建模
考虑离散时间线性动态系统：
x k = F k − 1 x k − 1 + B k − 1 u k − 1 + w k − 1 \mathbf{x}_k = \mathbf{F}_{k-1} \mathbf{x}_{k-1} + \mathbf{B}_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} xk=Fk−1xk−1+Bk−1uk−1+wk−1
z k = H k x k + v k \mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k zk=Hkxk+vk
其中：
- x k ∈ R n \mathbf{x}_k \in \mathbb{R}^n xk∈Rn 是系统的状态向量（例如物体位置、速度等）。
- F k \mathbf{F}_k Fk 是状态转移矩阵，描述状态如何从 k − 1 k-1 k−1 时刻演化到 k k k 时刻。
- B k \mathbf{B}_k Bk 是控制矩阵，映射外部控制输入 u k \mathbf{u}_k uk 到状态空间。
- w k ∼ N ( 0 , Q k ) \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k) wk∼N(0,Qk) 是过程噪声，其协方差矩阵为 Q k \mathbf{Q}_k Qk。
- z k ∈ R m \mathbf{z}_k \in \mathbb{R}^m zk∈Rm 是测量值，通过测量矩阵 H k \mathbf{H}_k Hk 从状态 x k \mathbf{x}_k xk 映射而来。
- v k ∼ N ( 0 , R k ) \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k) vk∼N(0,Rk) 是测量噪声，其协方差矩阵为 R k \mathbf{R}_k Rk。
假设：
- 状态噪声
w
k
\mathbf{w}_k
wk 和测量噪声
v
k
\mathbf{v}_k
vk 互不相关：
E [ w i v j T ] = 0 , ∀ i , j \mathbb{E}[\mathbf{w}_i \mathbf{v}_j^T] = 0, \quad \forall i,j E[wivjT]=0,∀i,j - 初始状态
x
0
\mathbf{x}_0
x0 服从高斯分布：
x 0 ∼ N ( x ^ 0 , P 0 ) \mathbf{x}_0 \sim \mathcal{N}(\hat{\mathbf{x}}_0, \mathbf{P}_0) x0∼N(x^0,P0)
2. 预测步骤（先验估计）
我们希望根据 k − 1 k-1 k−1 时刻的状态估计 x ^ k − 1 \hat{\mathbf{x}}_{k-1} x^k−1 和协方差 P k − 1 \mathbf{P}_{k-1} Pk−1，预测当前状态 x k \mathbf{x}_k xk 的分布。
状态预测（State Prediction）
x ^ k ∣ k − 1 = F k − 1 x ^ k − 1 + B k − 1 u k − 1 \hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_{k-1} \hat{\mathbf{x}}_{k-1} + \mathbf{B}_{k-1} \mathbf{u}_{k-1} x^k∣k−1=Fk−1x^k−1+Bk−1uk−1
协方差预测（Covariance Prediction）
P k ∣ k − 1 = F k − 1 P k − 1 F k − 1 T + Q k − 1 \mathbf{P}_{k|k-1} = \mathbf{F}_{k-1} \mathbf{P}_{k-1} \mathbf{F}_{k-1}^T + \mathbf{Q}_{k-1} Pk∣k−1=Fk−1Pk−1Fk−1T+Qk−1
3. 更新步骤（后验估计）
当新的测量 z k \mathbf{z}_k zk 到来后，我们需要结合预测值 x ^ k ∣ k − 1 \hat{\mathbf{x}}_{k|k-1} x^k∣k−1 和测量值 z k \mathbf{z}_k zk 进行修正，使最终估计更加准确。
计算创新（Innovation）
y ~ k = z k − H k x ^ k ∣ k − 1 \tilde{\mathbf{y}}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} y~k=zk−Hkx^k∣k−1
计算创新协方差（Innovation Covariance）
S k = H k P k ∣ k − 1 H k T + R k \mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k Sk=HkPk∣k−1HkT+Rk
计算卡尔曼增益（Kalman Gain）
K k = P k ∣ k − 1 H k T S k − 1 \mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1} Kk=Pk∣k−1HkTSk−1
更新状态估计
x ^ k = x ^ k ∣ k − 1 + K k y ~ k \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \tilde{\mathbf{y}}_k x^k=x^k∣k−1+Kky~k
更新协方差估计
P k = ( I − K k H k ) P k ∣ k − 1 \mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} Pk=(I−KkHk)Pk∣k−1
3.1 卡尔曼增益的推导过程
-
创新（创新）：
创新是观测值与预测值之间的差异，表示为：
y k = z k − H k x ^ k − \mathbf{y}_k = \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}^-_k yk=zk−Hkx^k−
创新反映了新观测对当前估计的修正。 -
后验协方差的计算：
后验协方差矩阵 ( P k ) (\mathbf{P}_k) (Pk)是基于预测协方差矩阵 ( P k − ) (\mathbf{P}^-_k) (Pk−)、卡尔曼增益 ( K k ) (\mathbf{K}_k) (Kk) 和观测模型 ( H k ) (\mathbf{H}_k) (Hk) 计算的。根据误差传递公式，后验协方差矩阵为：
P k = ( I − K k H k ) P k − \mathbf{P}_k = (I - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}^-_k Pk=(I−KkHk)Pk− -
卡尔曼增益的推导：
为了最小化后验误差的协方差，我们要求 ( P k ) (\mathbf{P}_k) (Pk) 最小化，因此需要对卡尔曼增益进行选择。通过最小化估计误差协方差，可以得到最优的卡尔曼增益 ( K k ) (\mathbf{K}_k) (Kk)。从最小化后验估计误差协方差的角度出发，我们要解决以下优化问题：
K k = arg min K k E [ ( x k − x ^ k ) ( x k − x ^ k ) ⊤ ] \mathbf{K}_k = \arg\min_{\mathbf{K}_k} \mathbb{E}[(\mathbf{x}_k - \hat{\mathbf{x}}_k)(\mathbf{x}_k - \hat{\mathbf{x}}_k)^\top] Kk=argKkminE[(xk−x^k)(xk−x^k)⊤]
经过推导，卡尔曼增益的表达式为：
K k = P k − H k ⊤ ( H k P k − H k ⊤ + R k ) − 1 \mathbf{K}_k = \mathbf{P}^-_k \mathbf{H}_k^\top (\mathbf{H}_k \mathbf{P}^-_k \mathbf{H}_k^\top + \mathbf{R}_k)^{-1} Kk=Pk−Hk⊤(HkPk−Hk⊤+Rk)−1
3.2. 卡尔曼增益的含义
- ( P k − ) (\mathbf{P}^-_k) (Pk−)： 预测的协方差矩阵，表示了预测状态的不确定性。
- ( H k ) (\mathbf{H}_k) (Hk)： 观测矩阵，表示如何从状态向量生成观测值。
- ( R k ) (\mathbf{R}_k) (Rk)： 观测噪声的协方差矩阵，表示观测噪声的大小。
卡尔曼增益 ( K k ) (\mathbf{K}_k) (Kk) 的作用是平衡预测误差和观测误差的贡献。在更新过程中，卡尔曼增益越大，表明观测对状态估计的修正作用越大；反之，卡尔曼增益越小，则表示系统对预测的依赖越强。
4. Python 实现示例
以下是一个一维位置估计的卡尔曼滤波示例：
import numpy as np
# 初始化变量
x = 0 # 初始状态
P = 1 # 初始不确定性
F = 1 # 状态转移矩阵
H = 1 # 观测矩阵
R = 1 # 观测噪声
Q = 0.1 # 过程噪声
# 测量值
measurements = [1, 2, 3]
for z in measurements:
# 预测
x = F * x
P = F * P * F + Q
# 计算卡尔曼增益
K = P * H / (H * P * H + R)
# 更新
x = x + K * (z - H * x)
P = (1 - K * H) * P
print(f'Updated State: {x}, Updated Covariance: {P}')
5. 卡尔曼滤波的优缺点
5.1 优点
- 最优性： 在满足线性系统模型和高斯噪声假设时，卡尔曼滤波可以提供最优的状态估计。
- 实时性： 卡尔曼滤波是一种递推算法，能够实时更新估计，不需要存储所有历史数据。
- 计算效率高： 卡尔曼滤波不需要复杂的计算，且可以通过简单的矩阵运算实现。
5.2 缺点
- 线性假设： 卡尔曼滤波假设系统是线性的，无法直接应用于非线性系统。如果系统具有非线性特征，需要使用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。
- 高斯噪声假设： 卡尔曼滤波假设噪声是高斯分布的，在实际应用中，噪声可能不符合该假设，导致滤波效果不理想。
- 参数选择： 卡尔曼滤波的性能对过程噪声
- ( Q ) (\mathbf{Q}) (Q) 和观测噪声 ( R ) (\mathbf{R}) (R) 的选择非常敏感，需要根据具体问题进行调节。
6. 卡尔曼滤波的应用
卡尔曼滤波在实际中有广泛的应用，尤其是在动态系统、自动控制和传感器融合等领域：
- 导航与定位： 在航天、汽车导航中，卡尔曼滤波用于GPS与惯性测量单元（IMU）融合，提供精确的位置信息。
- 自动驾驶： 在自动驾驶系统中，卡尔曼滤波常用于多传感器融合（如雷达、激光雷达和摄像头），实现精确的物体检测与路径规划。
- 机器人定位与路径规划： 卡尔曼滤波用于机器人中的定位、地图构建和自主导航。
- 金融工程： 在金融市场中，卡尔曼滤波用于时间序列预测和模型估计，处理金融数据中的噪声和不确定性。
7. 扩展卡尔曼滤波（EKF）与无迹卡尔曼滤波（UKF）
7.1 扩展卡尔曼滤波（EKF）
对于非线性系统，扩展卡尔曼滤波（EKF）通过对非线性系统进行泰勒展开，将系统线性化，从而应用卡尔曼滤波方法。EKF的核心在于计算系统状态和观测方程的雅可比矩阵。
7.2 无迹卡尔曼滤波（UKF）
无迹卡尔曼滤波（UKF）是一种更为精确的卡尔曼滤波扩展方法，不需要对非线性方程进行线性化，而是通过采样点（Sigma Points）来传播状态的不确定性，从而提高估计精度。
通过这些方法，卡尔曼滤波能够在复杂的动态系统中提供有效的状态估计，尤其是在系统模型和观测数据具有非线性特征时。