kalman滤波

 卡尔曼滤波的示例代码:

import numpy as np
import matplotlib.pyplot as plt

# z_mat:观测矩阵
# x_mat:初始状态
# p_mat:定义初始状态协方差矩阵,对初始状态的不确定性
# f_mat:状态转移矩阵
# q_mat:在跟踪任务当中，过程噪声来自于目标移动的不确定性（突然加速、减速、转弯等),白噪声
# h_mat:状态空间到测空间的关系，也就是量测方法的不确定性
# r_mat:观测噪声来自于检测框丢失、重叠等

# 创建一个0-99的一维矩阵
z = [i for i in range(100)]
z_watch = np.mat(z)
# print(z_mat)

# 创建一个方差为1的高斯噪声，精确到小数点后两位
noise = np.round(np.random.normal(0, 1, 100), 2)
noise_mat = np.mat(noise)

# 将z的观测值和噪声相加
z_mat = z_watch + noise_mat
# print(z_watch)

# 定义x的初始状态
x_mat = np.mat([[0, ], [0, ]])
# 定义初始状态协方差矩阵
p_mat = np.mat([[1, 0], [0, 1]])
# 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1
f_mat = np.mat([[1, 1], [0, 1]])
# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
# 定义观测矩阵
h_mat = np.mat([1, 0])
# 定义观测噪声协方差
r_mat = np.mat([1])

for i in range(100):
    # 状态转移矩阵*初始状态矩阵
    x_predict = f_mat * x_mat
    # f_mat状态转移矩阵  q_mat噪声协方差矩阵
    p_predict = f_mat * p_mat * f_mat.T + q_mat

    # 卡尔曼系数
    # h_mat:观测矩阵   r_mat:观测矩阵的误差
    kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
    # z_mat:观测矩阵
    x_mat = x_predict + kalman * (z_mat[0, i] - h_mat * x_predict)
    p_mat = (np.eye(2) - kalman * h_mat) * p_predict

    plt.plot(x_mat[0, 0], x_mat[1, 0], 'ro', markersize=1)

plt.show()