卡尔曼滤波器学习理解

dueen1123

已于 2024-10-21 16:05:49 修改

阅读量314

点赞数 1

文章标签：学习算法人工智能

于 2022-09-14 15:06:48 首次发布

本文链接：https://blog.csdn.net/dueen1123/article/details/126851331

版权

参考matlab官方讲解https://www.zhihu.com/question/23971601/answer/839664224

0.简明扼要的理解

这里的更新状态协方差矩阵P，测量噪声R，过程噪声Q初始值是怎么确定的尚不理解
在这里插入图片描述

代码如下

# 卡尔曼滤波预测类，用于预测 UAV 的位置和速度
# 测量值y只有位置，如[px, py, pz]
# 状态值x有位置和速度，如[px, py, pz, vx, vy, vz]
class KalmanFilterPredictor:
    def __init__(self, dt, process_noise=0.01, measurement_noise=5):
        self.x = None  # 初始状态（位置和速度）

        # 状态转移矩阵
        self.F = np.array([[1, 0, 0, dt, 0, 0],
                           [0, 1, 0, 0, dt, 0],
                           [0, 0, 1, 0, 0, dt],
                           [0, 0, 0, 1, 0, 0],
                           [0, 0, 0, 0, 1, 0],
                           [0, 0, 0, 0, 0, 1]], dtype=float)

        # 测量矩阵
        self.H = np.array([[1, 0, 0, 0, 0, 0],
                           [0, 1, 0, 0, 0, 0],
                           [0, 0, 1, 0, 0, 0]], dtype=float)

        # 初始状态协方差矩阵
        self.P = np.eye(6) * 1000

        # 测量噪声协方差矩阵
        self.R = np.eye(3) * measurement_noise

        # 过程噪声协方差矩阵
        self.Q = np.eye(6) * process_noise

        # 存储滤波后的位置和速度
        self.filtered_positions = []
        self.filtered_velocities = []

    def initialize_state(self, initial_position):
        # 初始化状态向量
        self.x = np.array([initial_position[0], initial_position[1], initial_position[2], 0, 0, 0], dtype=float)

    def predict(self, z, dt):
        # 更新时间间隔
        self.F[0, 3] = dt
        self.F[1, 4] = dt
        self.F[2, 5] = dt
        
        # 预测步骤
        self.x = np.dot(self.F, self.x)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q

        # 测量更新步骤
        y = np.array(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 = np.dot((np.eye(6) - np.dot(K, self.H)), self.P)

        # 存储结果
        self.filtered_positions.append((self.x[0], self.x[1], self.x[2]))
        self.filtered_velocities.append((self.x[3], self.x[4], self.x[5]))

1、卡尔曼滤波器

卡尔曼滤波器针对线性系统，可以解决基本的两种问题
（1）未知状态估计——状态观察器
（2）多状态估计最佳状态——最佳状态估计器

（1）状态观察器

作用：卡尔曼滤波器可以估计不能测量的状态
应用：火箭燃料内部测温

通过数学建模得到的测量值的估计值和测量值的误差来调整模型，从而得到较为准确的不可测量的估计值
在这里插入图片描述

数学建模和推导过程如下
在这里插入图片描述
x^为估计值；x·为倒数；u为系统输入；y为测量值
e为真实值和估计值的误差，是关于t的指数函数，证明错误会随着时间收敛，KC决定收敛速度

以上例子x为中间不可测量状态，y为可以测量状态，另外，当C=1时，可以演变为以下例子，也就是最佳状态估计器

（2）最佳状态估计器

作用：卡尔曼滤波器可以根据多个状态调整更精确的状态
应用：汽车GPS和里程计估计位置问题
在这里插入图片描述

y为测量值（GPS测量）；w为风产生的过程误差；v为GPS产生的随机误差（w和v都满足正态分布）
在这里插入图片描述
前半部分为先验估计；后半部分加上测量值的估计为后验估计

P为误差协方差，来自过程误差和预估值的不确定性
P- x-（-表示调整前）

测量值(y)可靠，则测量值对x^贡献更大（测量值相当于GPS测量位置）
预估值(x^ - )可靠，则预估值对x^贡献更大（预估值相当于里程计测量位置）
因此卡尔曼滤波器又称为传感器融合算法