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

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

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

先上卡尔曼滤波公式：

然后上代码：

# -*- coding: utf-8 -*
# 向下为正方向

import numpy as np
import matplotlib.pyplot as plt


def main():
    # 时间共1s，采样周期10ms
    dt = 0.01
    t = [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_mat
    v_z_mat = v_true_mat + v_noise_mat

    u_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_mat

        k = p_predict * h_mat.T * (h_mat * p_predict * h_mat.T + r_mat).I
        zt = 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_predict

        plt.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()

结果如下：

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