卡曼滤波算法 python

by AI

import numpy as np 
 
class KalmanFilter: 
    def __init__(self, A, H, Q, R, x0, P0): 
        """ 
        初始化卡尔曼滤波器 
        :param A: 状态转移矩阵 
        :param H: 观测矩阵 
        :param Q: 过程噪声协方差矩阵 
        :param R: 观测噪声协方差矩阵 
        :param x0: 初始状态估计 
        :param P0: 初始误差协方差矩阵 
        """ 
        self.A = A  # 状态转移矩阵 
        self.H = H  # 观测矩阵 
        self.Q = Q  # 过程噪声协方差矩阵 
        self.R = R  # 观测噪声协方差矩阵 
        self.x = x0  # 初始状态估计 
        self.P = P0  # 初始误差协方差矩阵 
 
    def predict(self): 
        """ 
        预测步骤 
        """ 
        self.x = np.dot(self.A,  self.x)  # 预测状态 
        self.P = np.dot(np.dot(self.A,  self.P), self.A.T) + self.Q  # 预测误差协方差 
 
    def update(self, z): 
        """ 
        更新步骤 
        :param z: 观测值 
        """ 
        y = z - np.dot(self.H,  self.x)  # 计算观测残差 
        S = np.dot(np.dot(self.H,  self.P), self.H.T) + self.R  # 计算残差协方差 
        K = np.dot(np.dot(self.P,  self.H.T), np.linalg.inv(S))   # 计算卡尔曼增益 
        self.x = self.x + np.dot(K,  y)  # 更新状态估计 
        self.P = self.P - np.dot(np.dot(K,  self.H), self.P)  # 更新误差协方差 
 
# 示例使用 
if __name__ == "__main__": 
    # 定义系统参数 
    A = np.array([[1,  1], [0, 1]])  # 状态转移矩阵 
    H = np.array([[1,  0]])  # 观测矩阵 
    Q = np.array([[0.0001,  0], [0, 0.0001]])  # 过程噪声协方差矩阵 
    R = np.array([[0.1]])   # 观测噪声协方差矩阵 
    x0 = np.array([0,  0])  # 初始状态估计 
    P0 = np.array([[1,  0], [0, 1]])  # 初始误差协方差矩阵 
 
    # 创建卡尔曼滤波器实例 
    kf = KalmanFilter(A, H, Q, R, x0, P0) 
 
    # 模拟观测数据 
    observations = [1.1, 2.1, 3.2, 4.0, 5.0] 
 
    # 运行卡尔曼滤波 
    for z in observations: 
        kf.predict()  
        kf.update(np.array([z]))  
        print(f"Estimated state: {kf.x}")