简述:
卡尔曼滤波(Kalman filtering)一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。
预备内容:
方差:各个样本与样本均值的差的平方和的均值,度量一组数据的分散程度。 协方差:两个变量总体误差的期望,度量两个变量线性相关性程度。 当两个变量是相同的情况,协方差就是方差。 如果两个变量的变化趋势相反,其中一个变量大于自身的期望值时,另外一个却小于自身的期望值,那么两个变量之间的协方差就是负值。 两者之间的联系或者关系,关系越大,协方差越大。 协方差矩阵:数据集中两两变量的协方差组成,每个元素是各个向量元素之间的协方差 噪声协方差矩阵越小说明噪声的误差越小,可信度越高,其对角线上的值就是方差。 误差协方差矩阵越小说明过程噪声和量测噪声的关系越小,用比例分开过程噪声ω 和 观测噪声υ,如果关系越小,分开的越精确。比如一堆白砂糖和盐,如果两种混合的很均匀,说它关系很大,也就越难用比例的方法将其分开。
协方差学习链接
线性系统
模型,运动方程和观测方程
其中,A为状态转移矩阵,B为可控的输入增益;W为运动模型系统的噪声(如导航系统里程计的测量噪声),V为观测系统噪声(如导航系统的相机观测噪声);
其中这两部分噪声符合高斯分布:
预测和更新
注: xk表示K时刻的状态, xk^帽子表示K时刻的优化预测状态(非真实状态); 而xk-表示K时刻的预测状态,该状态(先验)依据上一个状态的后验得来,之后在更新部分进行修正去掉-。其中 p-表示协方差矩阵(误差矩阵),当估计状态是1维数据时,若加入噪声,仅考虑方差 sita就可以;当估计状态是高维时,为了考虑各个维度偏离均值的程度,必须引入协方差矩阵sigma 。Kk为卡尔曼增益(观测权重);H为观测噪声;Q预测噪声协方差;R观测噪声协方差。
公式推导此处不展示,有兴趣这可参考文章底部资料了解(免费)
非线性系统:
模型,运动模型和观测方程
线性化(Taylor Series)
在x0处泰勒一阶展开
预测和更新:
总体流程
实践部分
code:
import numpy as np
import matplotlib.pyplot as plt
delta_t = 0.1 # 每秒钟采一次样
end_t = 7 # 时间长度
time_t = end_t * 10 # 采样次数
t = np.arange(0, end_t, delta_t) # 设置时间数组
u = 1 # 定义外界对系统的作用 加速度
x = 1 / 2 * u * t ** 2 # 实际真实位置
v_var = 1 # 测量噪声的方差
# 创建高斯噪声,精确到小数点后两位
v_noise = np.round(np.random.normal(0, v_var, time_t), 2)
X = np.mat([[0], [0]]) # 定义预测优化值的初始状态
v = np.mat(v_noise) # 定义测量噪声
z = x + v # 定义测量值(假设测量值=实际状态值+噪声)
A = np.mat([[1, delta_t], [0, 1]]) # 定义状态转移矩阵
B = [[1 / 2 * (delta_t ** 2)], [delta_t]] # 定义输入控制矩阵
P = np.mat([[1, 0], [0, 1]]) # 定义初始状态协方差矩阵
Q = np.mat([[0.001, 0], [0, 0.001]]) # 定义状态转移(预测噪声)协方差矩阵
H = np.mat([1, 0]) # 定义观测矩阵
R = np.mat([1]) # 定义观测噪声协方差
X_mat = np.zeros(time_t) # 初始化记录系统预测优化值的列表
for i in range(time_t):
# 预测
X_predict = A * X + np.dot(B, u) # 估算状态变量
P_predict = A * P * A.T + Q # 估算状态误差协方差
# 校正
K = P_predict * H.T / (H * P_predict * H.T + R) # 更新卡尔曼增益
X = X_predict + K * (z[0, i] - H * X_predict) # 更新预测优化值
P = (np.eye(2) - K * H) * P_predict # 更新状态误差协方差
# 记录系统的预测优化值
X_mat[i] = X[0, 0]
plt.rcParams['font.sans-serif'] = ['SimHei'] # 设置正常显示中文
plt.plot(x, "b", label='实际状态值') # 设置曲线数值
plt.plot(X_mat, "g", label='预测优化值')
plt.plot(z.T, "r--", label='测量值')
plt.xlabel("时间") # 设置X轴的名字
plt.ylabel("位移") # 设置Y轴的名字
plt.title("卡尔曼滤波示意图") # 设置标题
plt.legend() # 设置图例
plt.show() # 显示图表
注释详细,不做解释,对照上边五个公式。
运行结果:
理论推导以及python应用可参考(较为专业资源):
https://download.csdn.net/download/weixin_45080292/85418585
优秀博客推荐:
https://medium.com/@jaems33/understanding-kalman-filters-with-python-2310e87b8f48