简要介绍 | 多传感器融合中的卡尔曼滤波
1 卡尔曼滤波简介
卡尔曼滤波（Kalman filter） 是一种线性最优递归滤波器，用于估计动态系统的状态。它是在 1960 年由 Rudolf E. Kalman 提出的。卡尔曼滤波器主要用于带有高斯分布噪声的线性动态系统。它在预测系统状态和纠正预测状态方面有两个主要步骤。
2 公式描述
以下是卡尔曼滤波的基本公式：
设系统的动态模型和观测模型如下：
-
动态模型：描述系统状态随时间演变的模型。
x k = F k x k − 1 + B k u k + w k x_{k} = F_{k}x_{k-1} + B_{k}u_{k} + w_{k} xk=Fkxk−1+Bkuk+wk
其中，
- x k x_{k} xk 是系统在时刻 k k k 的状态向量。
- F k F_{k} Fk 是系统在时刻 k k k 的状态转换矩阵。
- x k − 1 x_{k-1} xk−1 是系统在时刻 k − 1 k-1 k−1 的状态向量。
- B k B_{k} Bk 是系统在时刻 k k k 的控制输入矩阵。
- u k u_{k} uk 是系统在时刻 k k k 的控制输入向量。
- w k w_{k} wk 是系统在时刻 k k k 的过程噪声向量，假设服从高斯分布， w k ∼ N ( 0 , Q k ) w_{k} \sim N(0, Q_{k}) wk∼N(0,Qk)，其中 Q k Q_{k} Qk 是过程噪声协方差矩阵。
-
观测模型：描述如何从系统状态获得观测量的模型。
z k = H k x k + v k z_{k} = H_{k}x_{k} + v_{k} zk=Hkxk+vk
其中，
- z k z_{k} zk 是系统在时刻 k k k 的观测向量。
- H k H_{k} Hk 是系统在时刻 k k k 的观测矩阵。
- x k x_{k} xk 是系统在时刻 k k k 的状态向量。
- v k v_{k} vk 是系统在时刻 k k k 的观测噪声向量，假设服从高斯分布， v k ∼ N ( 0 , R k ) v_{k} \sim N(0, R_{k}) vk∼N(0,Rk)，其中 R k R_{k} Rk 是观测噪声协方差矩阵。
卡尔曼滤波包括以下两个主要步骤：
-
预测：基于系统模型预测状态及其协方差。
x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k \begin{aligned} \hat{x}_{k|k-1} &= F_{k}\hat{x}_{k-1|k-1} + B_{k}u_{k} \\ P_{k|k-1} &= F_{k}P_{k-1|k-1}F_{k}^{T} + Q_{k} \end{aligned} x^k∣k−1Pk∣k−1=Fkx^k−1∣k−1+Bkuk=FkPk−1∣k−1FkT+Qk
其中， x ^ k ∣ k − 1 \hat{x}_{k|k-1} x^k∣k−1 是在时刻 k − 1 k-1 k−1 时关于时刻 k k k 的状态预测， P k ∣ k − 1 P_{k|k-1} Pk∣k−1 是预测状态协方差。
-
更新：根据观测值纠正预测状态。
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \begin{aligned} K_{k} &= P_{k|k-1}H_{k}^{T}(H_{k}P_{k|k-1}H_{k}^{T} + R_{k})^{-1} \\ \hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_{k}(z_{k} - H_{k}\hat{x}_{k|k-1}) \\ P_{k|k} &= (I - K_{k}H_{k})P_{k|k-1} \end{aligned} Kkx^k∣kPk∣k=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1=x^k∣k−1+Kk(zk−Hkx^k∣k−1)=(I−KkHk)Pk∣k−1
其中， x ^ k ∣ k \hat{x}_{k|k} x^k∣k 是在时刻 k k k 的状态估计值， P k ∣ k P_{k|k} Pk∣k 是估计状态协方差， K k K_{k} Kk 是卡尔曼增益。
3 应用领域
卡尔曼滤波器在多个领域有广泛的应用，包括：
- 导航系统：如全球定位系统（GPS）定位和惯性导航系统（INS）融合。
- 机器人：如在无人机、自主驾驶汽车和移动机器人中进行状态估计和传感器融合。
- 信号处理：如雷达和通信系统中的目标跟踪。
- 经济学：如时间序列分析和预测。
4 优缺点
卡尔曼滤波的优点：
- 最优性：对于线性动态系统和高斯噪声，卡尔曼滤波器是最优的。
- 实时性：卡尔曼滤波器是一种递归滤波器，不需要存储所有历史数据。
- 易于实现：卡尔曼滤波器的实现相对简单，只需要矩阵运算。
卡尔曼滤波的缺点：
- 局限性：仅适用于线性动态系统和高斯噪声。对于非线性系统，可以使用扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF）等变种。
- 对模型的依赖性：卡尔曼滤波器的性能依赖于系统模型的准确性。如果模型存在较大误差，滤波器的性能会受到影响。
- 初始条件敏感：卡尔曼滤波器对初始状态和协方差的选择敏感。如果初始条件选择不当，可能导致滤波器收敛速度较慢或不稳定。
4 自动驾驶多传感器融合中的卡尔曼滤波
4.1 传感器观测系统建模
假设我们有一个 自动驾驶系统 ，其状态向量 x k x_k xk 在时刻 k k k 由以下参数表示：
x k = [ p x p y v x v y a x a y ] x_k = \begin{bmatrix} p_x \ p_y \ v_x \ v_y \ a_x \ a_y \end{bmatrix} xk=[px py vx vy ax ay]
其中 p x p_x px 和 p y p_y py 为 位置 ， v x v_x vx 和 v y v_y vy 为 速度 ， a x a_x ax 和 a y a_y ay 为 加速度 。系统的动态模型可以表示为：
x k + 1 = F k x k + B k u k + w k x_{k+1} = F_k x_k + B_k u_k + w_k xk+1=Fkxk+Bkuk+wk
其中 F k F_k Fk 是状态转换矩阵， B k B_k Bk 是控制输入矩阵， u k u_k uk 是控制输入向量， w k w_k wk 是过程噪声向量。
传感器观测模型可以表示为：
z k ( i ) = H k ( i ) x k + v k ( i ) z_{k}^{(i)} = H_{k}^{(i)}x_k + v_{k}^{(i)} zk(i)=Hk(i)xk+vk(i)
其中 z k ( i ) z_k^{(i)} zk(i) 是第 i i i 个传感器在时刻 k k k 的观测向量， H k ( i ) H_k^{(i)} Hk(i) 是观测矩阵， v k ( i ) v_k^{(i)} vk(i) 是观测噪声向量。
4.2 卡尔曼滤波器的融合
卡尔曼滤波器 用于整合来自多个传感器的信息，估计自动驾驶系统的状态。以下是卡尔曼滤波器在多传感器融合中的应用步骤：
预测：对每个传感器，预测系统状态及其协方差。
x ^ k ∣ k − 1 ( i ) = F k ( i ) x ^ k − 1 ∣ k − 1 ( i ) + B k ( i ) u k ( i ) P k ∣ k − 1 ( i ) = F k ( i ) P k − 1 ∣ k − 1 ( i ) F k ( i ) T + Q k ( i ) \begin{aligned} \hat{x}{k|k-1}^{(i)} &= F{k}^{(i)}\hat{x}{k-1|k-1}^{(i)} + B{k}^{(i)}u_{k}^{(i)} \ P_{k|k-1}^{(i)} &= F_{k}^{(i)}P_{k-1|k-1}^{(i)}F_{k}^{(i)T} + Q_{k}^{(i)} \end{aligned} x^k∣k−1(i)=Fk(i)x^k−1∣k−1(i)+Bk(i)uk(i) Pk∣k−1(i)=Fk(i)Pk−1∣k−1(i)Fk(i)T+Qk(i)
其中， x ^ k ∣ k − 1 ( i ) \hat{x}{k|k-1}^{(i)} x^k∣k−1(i) 是第 i i i 个传感器在时刻 k − 1 k-1 k−1 时关于时刻 k k k 的状态预测， P k ∣ k − 1 ( i ) P{k|k-1}^{(i)} Pk∣k−1(i) 是预测状态协方差。
融合：整合来自所有传感器的预测状态和协方差，得到整体预测状态和协方差。
x ^ k ∣ k − 1 = ∑ i = 1 N W k ( i ) x ^ k ∣ k − 1 ( i ) P k ∣ k − 1 = ∑ i = 1 N W k ( i ) P k ∣ k − 1 ( i ) W k ( i ) T \begin{aligned} \hat{x}{k|k-1} &= \sum{i=1}^{N} W_{k}^{(i)} \hat{x}{k|k-1}^{(i)} \ P{k|k-1} &= \sum_{i=1}^{N} W_{k}^{(i)} P_{k|k-1}^{(i)} {W_{k}^{(i)}}^T \end{aligned} x^k∣k−1=∑i=1NWk(i)x^k∣k−1(i) Pk∣k−1=i=1∑NWk(i)Pk∣k−1(i)Wk(i)T
其中， W k ( i ) W_{k}^{(i)} Wk(i) 是第 i i i 个传感器在时刻 k k k 的权重，可以根据传感器的信噪比或其他指标计算。一种简单的权重计算方法是：
W k ( i ) = 1 P k ∣ k − 1 ( i ) W_{k}^{(i)} = \frac{1}{P_{k|k-1}^{(i)}} Wk(i)=Pk∣k−1(i)1
更新：使用观测数据更新整体状态和协方差。
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \begin{aligned} K_{k} &= P_{k|k-1}H_{k}^{T}(H_{k}P_{k|k-1}H_{k}^{T} + R_{k})^{-1} \\ \hat{x}_{k|k} &= \hat{x}_{k|k-1} + K_{k}(z_{k} - H_{k}\hat{x}_{k|k-1}) \\ P_{k|k} &= (I - K_{k}H_{k})P_{k|k-1} \end{aligned} Kkx^k∣kPk∣k=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1=x^k∣k−1+Kk(zk−Hkx^k∣k−1)=(I−KkHk)Pk∣k−1
其中， K k K_{k} Kk 是卡尔曼增益， R k R_{k} Rk 是观测噪声协方差， x ^ k ∣ k \hat{x}_{k|k} x^k∣k 是更新后的整体状态， P k ∣ k P_{k|k} Pk∣k 是更新后的整体状态协方差。
通过上述步骤，可以融合多个传感器的信息，估计自动驾驶系统的状态。卡尔曼滤波器在多传感器融合中的应用能有效地提高状态估计的准确性，从而提高自动驾驶系统的性能。
总结
通过使用卡尔曼滤波器进行多传感器融合，自动驾驶系统可以在实时和准确地估计其状态的同时，有效地整合不同传感器的信息。这种方法可以有效地处理传感器之间的不确定性和噪声，提高自动驾驶系统的环境感知和定位精度。