Kalman-Filter-Example
项目地址 https://github.com/zhengjie9510/kalman-filter-example
理论公式
详细理论可参考DR_CAN关于卡尔曼滤波器的视频讲解。https://www.bilibili.com/video/BV1dV411B7ME
卡尔曼滤波公式分为预测和更新两部分。
预测公式为:
x_hat_minus[k]= A * x_hat[k-1] + B * u[k-1]
P_minus[k] = A * P[k-1] * A.T + Q
更新公式为:
K[k] = P_minus[k] * H.T * (H * P_minus[k] * H.T + R).I
x_hat[k] = x_hat_minus[k] + K[k] * (z[k] - H * x_hat_minus[k])
P[k] = (np.eye(1) - K[k] * H) * P_minus[k]
其中:
x_hat_minus--先验估计状态
P_minus--先验估计协方差矩阵
x_hat--后验估计状态
P--后验估计协方差矩阵
A--状态转移矩阵
B--控制矩阵
u--控制量
Q--过程噪声协方差矩阵
K--卡尔曼增益
H--观测矩阵
R--观测噪声协方差矩阵
z--观测量。
应用
背景
假定有一个匀速运动的人,状态变量x1和x2,Xx1表示人的位置,x2表示人的速度,观测变量z1和z2,z1表示人的位置,z2表示人的速度。
状态转移方程如下:
速度:x2[k] = x2[k-1] + w2[k-1]
位置:x1[k] = x1[k-1] + x2[k-1] * dt + w1[k-1]
测量方程为:文章来源:https://www.toymoban.com/news/detail-719666.html
速度:z2[k] = x2[k] + v2[k]
位置:z1[k] = x1[k] + v1[k]
读取数据
df = pd.read_csv('data.csv', index_col=0)
x = df[['X1', 'X2']].values
z = df[['Z1', 'Z2']].values
设置参数并调用卡尔曼滤波函数
u = np.zeros(z.shape) # 控制量,当前场景为0
x_hat_first = np.array([[0, 1]]) # 初始估计
P_first = np.array([[1, 0], [0, 1]]) # 初始估计协方差
A = np.array([[1, 1], [0, 1]]) # 状态转移矩阵
B = np.array([[0, 0], [0, 0]]) # 控制矩阵
H = np.array([[1, 0], [0, 1]]) # 观测矩阵
Q = np.array([[0.1, 0], [0, 0.1]]) # 系统噪声协方差矩阵
R = np.array([[1, 0], [0, 1]]) # 测量噪声协方差矩阵
x_hat = kalman_filter(x, z, u, A, B, H, Q, R, x_hat_first, P_first)
结果展示
plt.figure(figsize=(16, 9))
plt.plot(x[:, 0], 'g--', label='position--noisy system estimate')
plt.plot(z[:, 0], 'k--', label='position--noisy measurements')
plt.plot(x_hat[:, 0], 'b--', label='position--a posteri estimate')
plt.legend()
plt.xlabel('Step')
plt.ylabel('Position')
plt.title('Position')
plt.show()
plt.figure(figsize=(16, 9))
plt.plot(x[:, 1], 'g-', label='speed--noisy system estimate')
plt.plot(z[:, 1], 'k-', label='speed--noisy measurements')
plt.plot(x_hat[:, 1], 'b-', label='speed--a posteri estimate')
plt.legend()
plt.xlabel('Step')
plt.ylabel('Speed')
plt.title('Speed')
plt.show()
文章来源地址https://www.toymoban.com/news/detail-719666.html
到了这里,关于卡尔曼滤波(Kalman Filter)原理及Python实现的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!