卡尔曼滤波简单实现运动模型

1.理论讲解参考这篇文章，比较通俗易懂：卡尔曼滤波讲解
核心公式如下，对着公式撸代码即可：

2.直接上代码：
中间随意设置了一组数据，x,y的观测值，两个方向速度都设为1，时间间隔为1s。效果图如下，预测值波动比较大，KF滤波后比较平滑，和观测值比较接近：

代码如下：

import matplotlib.pyplot as plt
import numpy as np
class Kf_Params():
    B = 0  # 外部输入为0
    u = 0  # 外部输入为0
    K = float('nan')  # 卡尔曼增益无需初始化
    z = float('nan')  # 这里无需初始化，每次使用kf_update之前需要输入观察值z
    P = np.diag(np.ones(4))  # 初始P设为ones(4, 4)

    # 初始状态：函数外部提供初始化的状态，本例使用观察值进行初始化，vx，vy初始为1
    x = []
    G = []
    #状态转移矩阵F，时间间隔为1s
    F = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)
    #预测噪声协方差矩阵
    Q = np.diag(np.ones(4)) * 0.1
    H = np.eye(2, 4)
    # 观测噪声协方差矩阵R
    R = np.diag(np.ones(2)) * 0.1
def kf_init(px, py, vx, vy):
    # 状态x为（坐标x， 坐标y， 速度x， 速度y），观测值z为（坐标x， 坐标y）
    kf_params = Kf_Params()
    kf_params.B = 0
    kf_params.u = 0
    kf_params.K = float('nan')
    kf_params.z = float('nan')
    kf_params.P = np.diag(np.ones(4))
    kf_params.x = [px, py, vx, vy]
    kf_params.G = [px, py, vx, vy]
    kf_params.F= np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)
    kf_params.Q = np.diag(np.ones(4)) * 0.1
    kf_params.H = np.eye(2, 4)
    kf_params.R = np.diag(np.ones(2)) * 0.1
    return kf_params

def kf_update(kf_params):
    # 以下为卡尔曼滤波的五个方程（步骤）
    x_ = np.dot(kf_params.F, kf_params.x)+kf_params.B * kf_params.u
    p_ = np.dot(np.dot(kf_params.F, kf_params.P),np.transpose(kf_params.F))+kf_params.Q
    S=np.dot(np.dot(kf_params.H,p_),np.transpose(kf_params.H))+kf_params.R
    kf_params.K=np.dot(np.dot(p_,np.transpose(kf_params.H)),np.linalg.matrix_power(S, -1))
    y=kf_params.z-np.dot(kf_params.H,x_)
    kf_params.x=x_+np.dot(kf_params.K,y)#滤波值
    kf_params.P=p_-np.dot(np.dot(kf_params.K,kf_params.H),p_)
    kf_params.G = x_#预测值
    return kf_params
def accuracy(predictions, labels):
    return np.array(predictions) - np.array(labels)

if __name__=='__main__':
    x=np.linspace(0,30,200)
    y=x**2-2*x+np.random.randint(1, 100,200)#
    kf_params_record = np.zeros((len(y), 4))
    kf_params_p = np.zeros((len(y), 4))
    t = len(y)
    kalman_filter_params = kf_init(x[0], y[0], 1, 1)
    for i in range(t):
        if i==0:
            kalman_filter_params = kf_init(x[0], y[0], 1, 1)#初始化
        else:
            kalman_filter_params.z = np.transpose([x[i], y[i]])#设置当前时刻的观测位置
            kalman_filter_params = kf_update(kalman_filter_params) # 卡尔曼滤波
        kf_params_record[i, ::] = np.transpose(kalman_filter_params.x)
        kf_params_p[i, ::] = np.transpose(kalman_filter_params.G)
    kf_trace = kf_params_record[::, :2]
    kf_trace_1 = kf_params_p[::, :2]
    plt.figure()
    plt.plot(x, y, 'g')
    plt.plot(kf_trace[::, 0], kf_trace[::, 1], 'r')
    # plt.plot(kf_trace_1[::, 0], kf_trace_1[::, 1], 'b')
    legend = ["detection", "KF","predict"]
    plt.legend(legend, loc="best", frameon=False)
    plt.title('KF result')
    plt.show()