简单上手的卡尔曼滤波代码调用

---

前言

卡尔曼滤波是一种在随机环境中对动态系统进行最优估计的经典方法，利用系统的预测值和观测值，通过递归的方式，对系统状态进行准确估计。

为了能够更加直观的观察与使用卡尔曼滤波，本文记录了在python中直接调用卡尔曼滤波的接口，并以代码的形式详解各个变量的含义与调节经验。关于卡尔曼滤波的原理、数学推导等内容我这里就不介绍了，网上有很多的资源。好了，现在就开始吧~

---

一、背景

我使用的数据是一堆二维坐标，存放在了本地csv文件中，内容的含义是我每隔1s记录一次小车在匀速运动过程中的路线坐标，所有数据点都是按时间顺序排列的，并存放到文件中。然后读取这个文件进行滤波处理，旨在降低噪声影响，实现数据平滑。

二、先上代码

首先在对卡尔曼滤波有了一个初步的了解之后，这里就直接贴上一个常规的代码，针对代码进行详细介绍内部所涉及的各个参数。

import pandas as pd  
from pykalman import KalmanFilter  
import numpy as np
import matplotlib.pyplot as plt  
  
# 读取csv文件  
df = pd.read_csv('data.csv')  

'''
文件内的数据格式
x,y
2.253,1.634
2.641,2.103
...,...
...,...

'''
  
# x和y数据  
x_data = df['x'].values  
y_data = df['y'].values  
  
# 定义初始状态：我们假设初始位置（x0, y0）和速度（vx0, vy0）都为零  
# 这些值应该根据具体场景进行调整  
initial_state_mean = [x_data[0], 0, y_data[0], 0]  # [x position, x velocity, y position, y velocity]  
transition_matrices = [  
    [1, 1, 0, 0],  # x position = previous x position + x velocity  
    [0, 1, 0, 0],  # x velocity  
    [0, 0, 1, 1],  # y position = previous y position + y velocity  
    [0, 0, 0, 1]   # y velocity  
]  
  
# 卡尔曼滤波器的初始化  
kf = KalmanFilter(  
    initial_state_mean=initial_state_mean,  
    transition_matrices=transition_matrices,  
    observation_matrices=[[1, 0, 0, 0], [0, 0, 1, 0]],  # 观测矩阵：x位置和y位置  
    # 其他参数（如观测噪声、过程噪声等）需要根据数据进行调整  
    observation_covariance=np.eye(2) * 0.1,  # 观测噪声协方差  
    transition_covariance=np.eye(4) * 0.01  # 过程噪声协方差  
)  
  
# 应用卡尔曼滤波器  
state_means, state_covariances = kf.filter(np.vstack([x_data, y_data]).T)  
  
# 提取平滑后的x和y坐标  
smoothed_x = state_means[:, 0]  
smoothed_y = state_means[:, 2]  
  
# 输出或使用平滑后的数据  
print("Smoothed x coordinates:", smoothed_x)  
print("Smoothed y coordinates:", smoothed_y)

三、参数详解

系统状态的初始估计的均值（x）

initial_state_mean = [x_data[0], 0, y_data[0], 0] # [x position, x velocity, y position, y velocity]

含义：用于初始化卡尔曼滤波器的状态向量的均值。这个向量包含了需要跟踪、估计的系统状态参数，在这里是位置和速度。初始位置设置为数据中的第一个点，x，y方向的初始速度为0。

在计算过程中，位置和速度的变换主要取决于系统的状态转移矩阵（Transition Matrices）和观测矩阵（Observation Matrices）。状态转移矩阵描述了系统从一个时间步到下一个时间步状态如何变化，这里就是基于物体的当前位置和速度来预测下一个时间步的位置和速度。而观测矩阵则描述了如何从系统的状态空间映射到观测空间，这里就是基于物体的实际位置和速度来生成我们可以观测到的位置数据。

状态转移矩阵（Transition Matrices, F）

transition_matrices = [
[1, 1, 0, 0], # x position = previous x position + x velocity
[0, 1, 0, 0], # x velocity
[0, 0, 1, 1], # y position = previous y position + y velocity
[0, 0, 0, 1] # y velocity
]

含义：定义了卡尔曼滤波器状态是如何随时间变化。

• 第一行 [1, 1, 0, 0]：
  • 第一个元素 1 表示 x 位置（x_position）的新预测值是旧预测值加上 x 速度（x_velocity）的变化量。这里时间间隔为1，因此速度直接加到位置上。
  • 第二个元素 1 表示 x 速度的新预测值是旧的速度值（假设速度本身不受外部力影响，或外部力影响被简化为零）。这是一个简化的模型，实际应用中可能需要更复杂的模型来考虑加速度等因素。
  • 后两个元素 0 表示 x 位置和速度的预测不受 y 位置和速度的影响。
• 第二行 [0, 1, 0, 0]：
  • 这一行实际上是对第一行中 x 速度部分的细化，明确指出 x 速度的新预测仅依赖于旧的速度值（不受位置或其他方向速度影响）。
• 第三行 [0, 0, 1, 1]：
  • 类似于第一行，但应用于 y 坐标。第一个 1 表示 y 位置的新预测值是旧预测值加上 y 速度的变化量。
  • 第二个 1 表示 y 速度的新预测值是旧的速度值。
  • 后两个 0 表示 y 位置和速度的预测不受 x 位置和速度的影响。
• 第四行 [0, 0, 0, 1]：
  • 类似于第二行，但专注于 y 速度。表示 y 速度的新预测仅依赖于旧的速度值。

修改方法：如果记录的时间步长不是1（如每0.5秒更新一次），则需要将速度乘以相应的时间步长（例如，如果时间步长为0.5，则应将 [1, 1, 0, 0] 改为 [1, 0.5, 0, 0] ）。如果系统中有明显的加速度（如车辆加速、物体受重力影响等）或外力（如风、摩擦力等）影响，则需要在状态转移矩阵中考虑这些因素。这通常涉及到增加状态变量并修改状态转移矩阵以反映这些新变量如何影响位置和速度。

观测矩阵（Observation Matrices, H）

observation_matrices=[[1, 0, 0, 0], [0, 0, 1, 0]], # 观测矩阵：x位置和y位置

含义：用于将系统的内部状态空间转换到观测空间

在例子中，observation_matrices=[[1, 0, 0, 0], [0, 0, 1, 0]]，这意味着只观测到了物体的x和y位置，而没有直接观测到其速度。因此，观测矩阵中的每一行都对应一个观测变量（x位置和y位置），而每一列则对应状态向量中的一个分量（x位置、x速度、y位置和y速度）。由于我们不直接观测速度，所以速度对应的列（即第2列和第4列）在观测矩阵中的对应元素为0。

观测噪声协方差（Observation Noise Covariance, R）

observation_covariance=np.eye(2) * 0.1, # 观测噪声协方差

含义：这个参数代表了观测数据（即测量值）中噪声的大小。它通常是一个对角矩阵，也可以自定义，但要和观测向量的维度一致，这里是坐标x, y。即：如果测量是多维的，那么每个对角元素对应一个测量维度的噪声方差，一维则设置成常数。

由于观测向量是二维的（在这个例子中，包含 x 和 y 坐标），观测噪声协方差矩阵也必须是二维的，以反映这两个观测值之间的噪声关系。协方差矩阵的对角线元素表示每个观测值的噪声方差，而不是简单的对角线元素。

np.eye(2) * 0.1 中将单位矩阵的每个元素乘以 0.1，这实际上是在设置 x 和 y 观测值的噪声方差为 0.1。这意味着在每次观测中，我预期 x 和 y 的真实值与观测值之间的偏差的方差为0.1，通过调整0.1这个值，可以控制观测噪声的总体水平。

修改方法：如果可以获取历史数据，以此来估计每个测量维度的噪声方差进行设置，是最准确的。但这通常涉及到对多次测量数据的统计分析，以计算每个测量维度的方差。但若历史数据不可用或难以获取，只能根据经验选择一个相对合理的值。就需要通过试错法进行调整，以优化滤波器的性能。

过程噪声协方差（Process Noise Covariance, Q）

transition_covariance=np.eye(4) * 0.01 # 过程噪声协方差

含义：这个参数代表了系统状态转移过程中噪声的大小。它同样是一个矩阵，描述了系统状态在转移过程中可能受到的扰动。

过程噪声协方差的维度是由状态向量的维度决定的。状态向量包含了系统所有需要跟踪或估计的变量。本文例子中，状态向量包含了四个元素，位置（x和y坐标）和速度（x和y方向的速度）。因此，状态向量的维度是4。由于状态向量是四维的，过程噪声也必须是一个四维向量来匹配状态向量的维度。并且，该矩阵描述了噪声向量中各个元素之间的协方差关系，即每个状态变量上的噪声方差。

np.eye(4) * 0.01 创建了一个4x4的单位矩阵，并将每个元素乘以0.01。这实际上是在假设所有状态变量的过程噪声方差都是相等的，并且各状态变量之间的噪声是独立的（因为非对角线上的元素为0）。通过调整0.01这个值，可以控制过程噪声的总体水平。

修改方法：如果系统的动态变化非常平滑，如地面摩擦，风阻，等影响较小，那么过程噪声协方差可能相对较小；如果系统容易受到外部扰动，那么过程噪声协方差可能相对较大。除此以外，也可以通过试错法来确定。通过调整Q的值，观察滤波器的性能变化，找到最优的Q值。

---

总结

这是使用pykalman中的KalmanFilter进行实现的，但在filterpy.kalman包中也有KalmanFilter ，可以尝试使用不同的接口。本人也是刚接触，如有错误，欢迎交流。