# !/usr/bin/env python
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
font = {'family': 'SimSun', # 宋体
# 'weight': 'bold', # 加粗
'size': '10.5' # 五号
}
plt.rc('font', **font)
plt.rc('axes', unicode_minus=False)
# plt.rcParams['figure.facecolor'] = "#FFFFF0" # 设置窗体颜色
# plt.rcParams['axes.facecolor'] = "#FFFFF0" # 设置绘图区颜色
class Kf_Params:
B = 0 # 外部输入为0
u = 0 # 外部输入为0
K = float('nan') # 卡尔曼增益无需初始化
z = float('nan') # 这里无需初始化，每次使用kf_update之前需要输入观察值z
P = np.diag(np.ones(4)) # 初始P设为0 ??? zeros(4, 4)
# 初始状态：函数外部提供初始化的状态，本例使用观察值进行初始化，vx，vy初始为0
x = []
G = []
# 状态转移矩阵A
# 和线性系统的预测机制有关，这里的线性系统是上一刻的位置加上速度等于当前时刻的位置，而速度本身保持不变
A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)
# 预测噪声协方差矩阵Q：假设预测过程上叠加一个高斯噪声，协方差矩阵为Q
# 大小取决于对预测过程的信任程度。比如，假设认为运动目标在y轴上的速度可能不匀速，那么可以把这个对角矩阵
# 的最后一个值调大。有时希望出来的轨迹更平滑，可以把这个调更小
Q = np.diag(np.ones(4)) * 0.1
# 观测矩阵H：z = H * x
# 这里的状态是（坐标x， 坐标y， 速度x， 速度y），观察值是（坐标x， 坐标y），所以H = eye(2, 4)
H = np.eye(2, 4)
# 观测噪声协方差矩阵R：假设观测过程上存在一个高斯噪声，协方差矩阵为R
# 大小取决于对观察过程的信任程度。比如，假设观测结果中的坐标x值常常很准确，那么矩阵R的第一个值应该比较小
R = np.diag(np.ones(2)) * 0.1
def kf_init(px, py, vx, vy):
# 本例中，状态x为（坐标x， 坐标y， 速度x， 速度y），观测值z为（坐标x， 坐标y）
kf_params = Kf_Params()
kf_params.B = 0
kf_params.u = 0
kf_params.K = float('nan')
kf_params.z = float('nan')
kf_params.P = np.diag(np.ones(4))
kf_params.x = [px, py, vx, vy]
kf_params.G = [px, py, vx, vy]
kf_params.A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)
kf_params.Q = np.diag(np.ones(4)) * 0.1
kf_params.H = np.eye(2, 4)
kf_params.R = np.diag(np.ones(2)) * 0.1
return kf_params
def kf_update(kf_params):
# 以下为卡尔曼滤波的五个方程（步骤）
a1 = np.dot(kf_params.A, kf_params.x)
a2 = kf_params.B * kf_params.u
x_ = np.array(a1) + np.array(a2)
b1 = np.dot(kf_params.A, kf_params.P)
b2 = np.dot(b1, np.transpose(kf_params.A))
p_ = np.array(b2) + np.array(kf_params.Q)
c1 = np.dot(p_, np.transpose(kf_params.H))
c2 = np.dot(kf_params.H, p_)
c3 = np.dot(c2, np.transpose(kf_params.H))
c4 = np.array(c3) + np.array(kf_params.R)
c5 = np.linalg.matrix_power(c4, -1)
kf_params.K = np.dot(c1, c5)
d1 = np.dot(kf_params.H, x_)
d2 = np.array(kf_params.z) - np.array(d1)
d3 = np.dot(kf_params.K, d2)
kf_params.x = np.array(x_) + np.array(d3)
e1 = np.dot(kf_params.K, kf_params.H)
e2 = np.dot(e1, p_)
kf_params.P = np.array(p_) - np.array(e2)
kf_params.G = x_
return kf_params
def accuracy(predictions, labels):
return np.array(predictions) - np.array(labels)
if __name__ == '__main__':
# 真实路径
path = './9.xlsx'
data_A = pd.read_excel(path, header=None)
data_A_x = list(data_A.iloc[::, 0])
data_A_y = list(data_A.iloc[::, 1])
A = np.array(list(zip(data_A_x, data_A_y)))
# plt.subplot(131)
plt.figure()
plt.plot(data_A_x, data_A_y, 'b-+')
# plt.title('实际的真实路径')
# 检测到的路径
path = './10.xlsx'
data_B = pd.read_excel(path, header=None)
data_B_x = list(data_B.iloc[::, 0])
data_B_y = list(data_B.iloc[::, 1])
B = np.array(list(zip(data_B_x, data_B_y)))
# plt.subplot(132)
plt.plot(data_B_x, data_B_y, 'r-+')
# plt.title('检测到的路径')
# 卡尔曼滤波
kf_params_record = np.zeros((len(data_B), 4))
kf_params_p = np.zeros((len(data_B), 4))
t = len(data_B)
kalman_filter_params = kf_init(data_B_x[0], data_B_y[0], 0, 0)
for i in range(t):
if i == 0:
kalman_filter_params = kf_init(data_B_x[i], data_B_y[i], 0, 0) # 初始化
else:
# print([data_B_x[i], data_B_y[i]])
kalman_filter_params.z = np.transpose([data_B_x[i], data_B_y[i]]) # 设置当前时刻的观测位置
kalman_filter_params = kf_update(kalman_filter_params) # 卡尔曼滤波
kf_params_record[i, ::] = np.transpose(kalman_filter_params.x)
kf_params_p[i, ::] = np.transpose(kalman_filter_params.G)
kf_trace = kf_params_record[::, :2]
kf_trace_1 = kf_params_p[::, :2]
# plt.subplot(133)
plt.plot(kf_trace[::, 0], kf_trace[::, 1], 'g-+')
plt.plot(kf_trace_1[1:26, 0], kf_trace_1[1:26, 1], 'm-+')
legend = ['CMA最佳路径数据集', '检测路径', '卡尔曼滤波结果', '预测路径']
plt.legend(legend, loc="best", frameon=False)
plt.title('卡尔曼滤波后的效果')
plt.savefig('result.svg', dpi=600)
plt.show()
# plt.close()
p = accuracy(kf_trace, A)
print(p)
?
