kalman的python实现

前面的kalman都是matlab的，这里在理解的基础上，尝试使用python实现，力求理解更多的内涵。

需要的包
import numpy as np
import matplotlib.pyplot as plt

代码

KF algorith demo by Leo

2020.01.06

ZJG CAMPUS,ZJU

import numpy as np
import matplotlib.pyplot as plt

‘’’
生成带噪声的传感器观测值Z
Z中一共包含500个samples，第k个sample代表k时刻传感器的读数
假设只对机器人位置进行传感器观测，并且只用距离表示位置
因此，Z中只有一个观测变量，即机器人的位置，这个位置一维数据表示
‘’’

生成不带噪声的数据

Z_raw = [i for i in range(500)]

创建一个均值为0，方差为1的高斯噪声，共有500个samples，精确到小数点后两位

noise = np.round(np.random.normal(0, 1, 500), 2)

normal函数，后面三个值分别为平均数、标准差、数量，normal(x,n)表示将x四舍五入到小数点后n个数字

将z的观测值和噪声相加

Z = np.mat(Z_raw) + np.mat(noise)

‘’’
定义状态向量X的初始状态
X中包含两个状态变量：p和v，二者都被初始化为0，且二者都用标量表示
‘’’
X = np.mat([[0,], [0,]])

‘’’
定义初始状态协方差矩阵P
‘’’
P = np.mat([[1, 0], [0, 1]])

‘’’
定义状态转移矩阵F，假设每秒钟采一次样，所以delta_t = 1
‘’’
F = np.mat([[1, 1], [0, 1]])

‘’’
定义状态转移协方差矩阵Q
这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
‘’’
Q = np.mat([[0.0001, 0], [0, 0.0001]])

‘’’
定义观测矩阵H
‘’’
H = np.mat([1, 0])

‘’’
定义观测噪声协方差R
‘’’
R = np.mat([1])

‘’’
卡尔曼滤波算法的预测和更新过程
‘’’
for i in range(100):
x_predict = F * X#demo中没有引入控制矩阵B
p_predict = F * P * F.T + Q # 这里，F.T就是FT
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
print(X)
plt.plot(X[0, 0], X[1, 0], ‘ro’, markersize = 4)

plt.show()