使用卡尔曼滤波估计自由落体时的位置和速度

问题：通过传感器可以测量出自由落体时的加速度、速度和位置，通过卡尔曼滤波估计速度和位置。

坐标系：向下为正，初始位置和速度都为0

先上卡尔曼滤波公式：

然后上代码：

# -*- coding: utf-8 -*
# 向下为正方向import numpy as np
import matplotlib.pyplot as pltdef main():# 时间共1s，采样周期10msdt = 0.01t = [i * dt for i in range(0, 100)]g = 9.8# 真实值x_true_mat = np.mat(0.5 * g * np.multiply(np.array(t), np.array(t)))v_true_mat = g * np.mat(t)u_true_mat = np.mat([g for i in range(0, 100)])# 噪声x_noise = np.round(np.random.normal(0, 0.1, 100), 2)v_noise = np.round(np.random.normal(0, 0.1, 100), 2)u_noise = np.round(np.random.normal(0, 0.01, 100), 2)x_noise_mat = np.mat(x_noise)v_noise_mat = np.mat(v_noise)u_noise_mat = np.mat(u_noise)# 测量值x_z_mat = x_true_mat + x_noise_matv_z_mat = v_true_mat + v_noise_matu_mat = u_true_mat + u_noise_mat# 定义x的初始状态x_mat = np.mat([[0], [0]])# 定义初始状态协方差矩阵p_mat = np.mat([[1, 0], [0, 1]])# 状态转移矩阵f_mat = np.mat([[1, dt], [0, 1]])# 控制矩阵b_mat = np.mat([[0.5 * dt * dt], [dt]])# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高q_mat = np.mat([[1.0 * 1.0 * dt * dt, 0], [0, 1.0 * 1.0 * dt * dt]])# 定义观测矩阵h_mat = np.mat([[1, 0], [0, 1]])# 定义观测噪声协方差r_mat = np.mat([[1.0 * 1.0, 0], [0, 2.5 * 2.5]])for i in range(100):x_predict = f_mat * x_mat + b_mat * u_mat[0, i]p_predict = f_mat * p_mat * f_mat.T + q_matk = p_predict * h_mat.T * (h_mat * p_predict * h_mat.T + r_mat).Izt = np.mat([[x_z_mat[0, i]], [v_z_mat[0, i]]])x_mat = x_predict + k * (zt - h_mat * x_predict)p_mat = (p_mat - k * h_mat) * p_predictplt.plot(t[i], x_z_mat[0, i], 'ro', markersize=1)plt.plot(t[i], v_z_mat[0, i], 'ro', markersize=1)plt.plot(t[i], x_mat[0, 0], 'bo', markersize=1)plt.plot(t[i], x_mat[1, 0], 'bo', markersize=1)plt.show()if __name__ == '__main__':main()

结果如下：

图中的红色点分别是观测的位置和速度，蓝色点为估计出的位置和速度。