卡尔曼滤波器算法

原创

贺公子之数据科学与艺术 2025-05-22 09:38:00 ©著作权

文章标签 协方差卡尔曼滤波初始状态 文章分类 JavaScript 前端开发

©著作权归作者所有：来自51CTO博客作者贺公子之数据科学与艺术的原创作品，请联系作者获取转载授权，否则将追究法律责任

卡尔曼滤波器是一种用于估计动态系统状态的数学算法，尤其适用于具有噪声的线性系统。它在信号处理、控制工程、计算机视觉和导航系统等领域应用广泛。

基本原理

卡尔曼滤波器通过两步循环操作对系统的状态进行估计：

预测（Prediction）：根据系统模型预测当前状态和协方差。
更新（Correction）：利用测量值更新预测结果，从而获得更准确的估计。

其核心在于结合了系统模型的预测信息和观测值，通过加权方式平滑误差。

数学模型

状态方程

[ x_k = F_k x_{k-1} + B_k u_k + w_k ]

(x_k)：状态向量
(F_k)：状态转移矩阵
(B_k)：控制输入矩阵
(u_k)：控制输入
(w_k)：过程噪声（服从高斯分布）

观测方程

[ z_k = H_k x_k + v_k ]

(z_k)：观测向量
(H_k)：观测矩阵
(v_k)：观测噪声（服从高斯分布）

滤波过程

1. 初始化

初始化系统状态和协方差矩阵：

初始状态 (x_0)
初始协方差 (P_0)

2. 预测步骤

根据系统模型预测下一状态和误差协方差： [ \hat{x}k^- = F_k \hat{x}{k-1} + B_k u_k ] [ P_k^- = F_k P_{k-1} F_k^T + Q_k ]

(\hat{x}_k^-)：预测的状态
(P_k^-)：预测的协方差
(Q_k)：过程噪声协方差

3. 更新步骤

结合观测值进行更新： [ K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R_k)^{-1} ] [ \hat{x}_k = \hat{x}_k^- + K_k (z_k - H_k \hat{x}_k^-) ] [ P_k = (I - K_k H_k) P_k^- ]

(K_k)：卡尔曼增益
(R_k)：观测噪声协方差

循环执行预测和更新步骤，即完成滤波过程。

Python 实现

以下是卡尔曼滤波器的简单实现：

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R, P, x0):
        """
        F: 状态转移矩阵
        H: 观测矩阵
        Q: 过程噪声协方差
        R: 测量噪声协方差
        P: 初始状态协方差
        x0: 初始状态
        """
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.P = P
        self.x = x0

    def predict(self):
        """预测步骤"""
        self.x = np.dot(self.F, self.x)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q

    def update(self, z):
        """更新步骤"""
        y = z - np.dot(self.H, self.x)  # 观测残差
        S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R  # 残差协方差
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))  # 卡尔曼增益
        self.x += np.dot(K, y)  # 更新状态
        I = np.eye(self.F.shape[1])
        self.P = np.dot(I - np.dot(K, self.H), self.P)  # 更新协方差

    def get_state(self):
        """返回当前状态"""
        return self.x

使用示例

以下是一个简单的使用示例，假设我们估计一个一维匀速直线运动：

# 初始化
F = np.array([[1, 1], [0, 1]])  # 状态转移矩阵
H = np.array([[1, 0]])          # 观测矩阵
Q = np.array([[1e-5, 0], [0, 1e-5]])  # 过程噪声协方差
R = np.array([[1]])             # 测量噪声协方差
P = np.eye(2)                   # 初始状态协方差
x0 = np.array([0, 1])           # 初始状态

kf = KalmanFilter(F, H, Q, R, P, x0)

# 模拟观测数据
observations = [0.39, 0.50, 0.48, 0.29, 0.25]

for z in observations:
    kf.predict()
    kf.update(np.array([z]))
    print("Updated state:", kf.get_state())