卡尔曼滤波的Python实现

为了在Python编程环境下实现卡尔曼滤波算法,特编写此程序

主要用到了以下3个模块

  1. numpy(数学计算)
  2. pandas(读取数据)
  3. matplotlib(画图展示)

代码的核心是实现了一个Kf_Params类,该类定义了卡尔曼滤波算法的相关参数

然后是实现了一个kf_init()函数,用来初始化卡尔曼滤波算法的相关参数

接着实现了一个kf_update()函数,用来更新卡尔曼滤波算法的相关参数

最后在主程序中读取数据,并调用卡尔曼滤波算法预测数据

数据样例见评论区的网盘链接,完整代码如下:

# !/usr/bin/env python
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

font = {'family': 'SimSun',  # 宋体
        # 'weight': 'bold',  # 加粗
        'size': '10.5'  # 五号
        }
plt.rc('font', **font)
plt.rc('axes', unicode_minus=False)


# plt.rcParams['figure.facecolor'] = "#FFFFF0"  # 设置窗体颜色
# plt.rcParams['axes.facecolor'] = "#FFFFF0"  # 设置绘图区颜色

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设为0 ??? zeros(4, 4)

    # 初始状态:函数外部提供初始化的状态,本例使用观察值进行初始化,vx,vy初始为0
    x = []
    G = []

    # 状态转移矩阵A
    # 和线性系统的预测机制有关,这里的线性系统是上一刻的位置加上速度等于当前时刻的位置,而速度本身保持不变
    A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)

    # 预测噪声协方差矩阵Q:假设预测过程上叠加一个高斯噪声,协方差矩阵为Q
    # 大小取决于对预测过程的信任程度。比如,假设认为运动目标在y轴上的速度可能不匀速,那么可以把这个对角矩阵
    # 的最后一个值调大。有时希望出来的轨迹更平滑,可以把这个调更小
    Q = np.diag(np.ones(4)) * 0.1

    # 观测矩阵H:z = H * x
    # 这里的状态是(坐标x, 坐标y, 速度x, 速度y),观察值是(坐标x, 坐标y),所以H = eye(2, 4)
    H = np.eye(2, 4)

    # 观测噪声协方差矩阵R:假设观测过程上存在一个高斯噪声,协方差矩阵为R
    # 大小取决于对观察过程的信任程度。比如,假设观测结果中的坐标x值常常很准确,那么矩阵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.A = 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):
    # 以下为卡尔曼滤波的五个方程(步骤)
    a1 = np.dot(kf_params.A, kf_params.x)
    a2 = kf_params.B * kf_params.u
    x_ = np.array(a1) + np.array(a2)

    b1 = np.dot(kf_params.A, kf_params.P)
    b2 = np.dot(b1, np.transpose(kf_params.A))
    p_ = np.array(b2) + np.array(kf_params.Q)

    c1 = np.dot(p_, np.transpose(kf_params.H))
    c2 = np.dot(kf_params.H, p_)
    c3 = np.dot(c2, np.transpose(kf_params.H))
    c4 = np.array(c3) + np.array(kf_params.R)
    c5 = np.linalg.matrix_power(c4, -1)
    kf_params.K = np.dot(c1, c5)

    d1 = np.dot(kf_params.H, x_)
    d2 = np.array(kf_params.z) - np.array(d1)
    d3 = np.dot(kf_params.K, d2)
    kf_params.x = np.array(x_) + np.array(d3)

    e1 = np.dot(kf_params.K, kf_params.H)
    e2 = np.dot(e1, p_)
    kf_params.P = np.array(p_) - np.array(e2)

    kf_params.G = x_
    return kf_params


def accuracy(predictions, labels):
    return np.array(predictions) - np.array(labels)


if __name__ == '__main__':
    # 真实路径
    path = './9.xlsx'
    data_A = pd.read_excel(path, header=None)
    data_A_x = list(data_A.iloc[::, 0])
    data_A_y = list(data_A.iloc[::, 1])
    A = np.array(list(zip(data_A_x, data_A_y)))

    # plt.subplot(131)
    plt.figure()
    plt.plot(data_A_x, data_A_y, 'b-+')
    # plt.title('实际的真实路径')

    # 检测到的路径
    path = './10.xlsx'
    data_B = pd.read_excel(path, header=None)
    data_B_x = list(data_B.iloc[::, 0])
    data_B_y = list(data_B.iloc[::, 1])
    B = np.array(list(zip(data_B_x, data_B_y)))

    # plt.subplot(132)
    plt.plot(data_B_x, data_B_y, 'r-+')
    # plt.title('检测到的路径')

    # 卡尔曼滤波
    kf_params_record = np.zeros((len(data_B), 4))
    kf_params_p = np.zeros((len(data_B), 4))
    t = len(data_B)
    kalman_filter_params = kf_init(data_B_x[0], data_B_y[0], 0, 0)
    for i in range(t):
        if i == 0:
            kalman_filter_params = kf_init(data_B_x[i], data_B_y[i], 0, 0)  # 初始化
        else:
            # print([data_B_x[i], data_B_y[i]])
            kalman_filter_params.z = np.transpose([data_B_x[i], data_B_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.subplot(133)
    plt.plot(kf_trace[::, 0], kf_trace[::, 1], 'g-+')
    plt.plot(kf_trace_1[1:26, 0], kf_trace_1[1:26, 1], 'm-+')
    legend = ['CMA最佳路径数据集', '检测路径', '卡尔曼滤波结果', '预测路径']
    plt.legend(legend, loc="best", frameon=False)
    plt.title('卡尔曼滤波后的效果')
    plt.savefig('result.svg', dpi=600)
    plt.show()
    # plt.close()

    p = accuracy(kf_trace, A)
    print(p)

 卡尔曼滤波处理结果如下:

卡尔曼滤波的Python实现_第1张图片

可以看到,通过卡尔曼滤波算法预测的数据与真实的数据相差不大,成功实现了该算法

更新:2022年11月19日

更新说明:

  1. 将三个init、update、accuracy三个函数放在对象KalmanFilter内
  2. 修改了一些有意义的变量名,方便理解卡尔曼滤波器工作过程
  3. 丰富了滤波器输出数据的精度评价表格
  4. 修改了一些注释
  5. 增加了一个导弹跟踪敌机的卡尔曼滤波实例

代码如下:

# !/usr/bin/env python
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd

font = {'family': 'SimSun',  # 宋体
        'weight': 'bold',  # 加粗
        'size': '10.5'  # 五号
        }
plt.rc('font', **font)
plt.rc('axes', unicode_minus=False)


plt.rcParams['figure.facecolor'] = "#FFFFF0"  # 设置窗体颜色
plt.rcParams['axes.facecolor'] = "#FFFFF0"  # 设置绘图区颜色

class KalmanFilter:
    B = 0  # 控制变量矩阵,初始化为0
    u = 0  # 状态控制向量,初始化为0
    K = float('nan')  # 卡尔曼增益无需初始化
    z = float('nan')  # 观测值无需初始化,由外界输入
    P = np.diag(np.ones(4))  # 先验估计协方差

    x = []  # 滤波器输出状态
    G = []  # 滤波器预测状态

    # 状态转移矩阵A,和线性系统的预测机制有关
    A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)

    # 噪声协方差矩阵Q,代表对控制系统的信任程度,预测过程上叠加一个高斯噪声,若希望跟踪的轨迹更平滑,可以调小
    Q = np.diag(np.ones(4)) * 0.1

    # 观测矩阵H:z = H * x,这里的状态是(坐标x, 坐标y, 速度x, 速度y),观察值是(坐标x, 坐标y)
    H = np.eye(2, 4)

    # 观测噪声协方差矩阵R,代表对观测数据的信任程度,观测过程上存在一个高斯噪声,若观测结果中的值很准确,可以调小
    R = np.diag(np.ones(2)) * 0.1

    def init(self, px, py, vx, vy):
        # 本例中,状态x为(坐标x, 坐标y, 速度x, 速度y),观测值z为(坐标x, 坐标y)
        self.B = 0
        self.u = 0
        self.K = float('nan')
        self.z = float('nan')
        self.P = np.diag(np.ones(4))
        self.x = [px, py, vx, vy]
        self.G = [px, py, vx, vy]
        self.A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)
        self.Q = np.diag(np.ones(4)) * 0.1
        self.H = np.eye(2, 4)
        self.R = np.diag(np.ones(2)) * 0.1

    def update(self):
        # Xk_ = Ak*Xk-1+Bk*Uk
        a1 = np.dot(self.A, self.x)
        a2 = self.B * self.u
        x_ = np.array(a1) + np.array(a2)
        self.G = x_

        # Pk_ = Ak*Pk-1*Ak'+Q
        b1 = np.dot(self.A, self.P)
        b2 = np.dot(b1, np.transpose(self.A))
        p_ = np.array(b2) + np.array(self.Q)

        # Kk = Pk_*Hk'/(Hk*Pk_*Hk'+R)
        c1 = np.dot(p_, np.transpose(self.H))
        c2 = np.dot(self.H, p_)
        c3 = np.dot(c2, np.transpose(self.H))
        c4 = np.array(c3) + np.array(self.R)
        c5 = np.linalg.matrix_power(c4, -1)
        self.K = np.dot(c1, c5)

        # Xk = Xk_+Kk(Zk-Hk*Xk_)
        d1 = np.dot(self.H, x_)
        d2 = np.array(self.z) - np.array(d1)
        d3 = np.dot(self.K, d2)
        self.x = np.array(x_) + np.array(d3)

        # Pk = Pk_-Kk*Hk*Pk_
        e1 = np.dot(self.K, self.H)
        e2 = np.dot(e1, p_)
        self.P = np.array(p_) - np.array(e2)

    def accuracy(self, predictions, labels):
        return np.array(predictions) / np.array(labels)


if __name__ == '__main__':
    # 读取真实路径数据(客观真实的数据,作为滤波器预测结果的对比标签)
    # 比如敌机的真实飞行轨迹
    path = './9.xlsx'
    label = pd.read_excel(path, header=None)
    label_x = list(label.iloc[::, 0])
    label_y = list(label.iloc[::, 1])
    label_data = np.array(list(zip(label_x, label_y)))

    # 读取检测路径数据(传感器检测到的原始数据,与真实值之间会存在误差,作为滤波器的输入)
    # 比如我方导弹获取的敌机飞行轨迹,只能获取到当前时刻之前的轨迹信息,而不能直接获取未来的轨迹
    path = './10.xlsx'
    detect = pd.read_excel(path, header=None)
    detect_x = list(detect.iloc[::, 0])
    detect_y = list(detect.iloc[::, 1])
    detect_data = np.array(list(zip(detect_x, detect_y)))

    # 可视化(对原始数据进行可视化)
    plt.figure()
    plt.plot(label_x, label_y, 'b-+')
    plt.plot(detect_x, detect_y, 'r-+')

    # 卡尔曼滤波(根据卡尔曼对当前时刻的预测数据和当前时刻的观测数据,尽可能地输出下一时刻接近真实数据的数据)
    # 实现对敌机未来飞行轨迹的估计,达到跟踪目标的效果
    t = len(detect_data)  # 处理时刻
    kf_data_filter = np.zeros((t, 4))  # 滤波数据
    kf_data_predict = np.zeros((t, 4))  # 预测数据

    # 初始化(创建滤波器,并初始化滤波器状态)
    kf = KalmanFilter()
    kf.init(detect_x[0], detect_y[0], 0, 0)

    # 滤波处理(依次读取每一时刻的数据,输入到卡尔曼滤波器,输出预测结果)
    for i in range(t):
        if i == 0:
            kf.init(detect_x[i], detect_y[i], 0, 0)  # 初始化
        else:
            kf.z = np.transpose([detect_x[i], detect_y[i]])  # 获取当前时刻的观测数据
            kf.update()  # 更新卡尔曼滤波器参数
        kf_data_filter[i, ::] = np.transpose(kf.x)
        kf_data_predict[i, ::] = np.transpose(kf.G)

    kf_filter = kf_data_filter[::, :2]
    kf_predict = kf_data_predict[::, :2]

    # 评价(计算卡尔曼滤波器的预测精度)
    precision_detect = kf.accuracy(detect_data, label_data)
    precision_filter = kf.accuracy(kf_filter, label_data)
    print("-"*100)
    print("%-4s \t %-20s \t %-20s \t %-20s \t %-20s " % (
        "time", "detect gap x", "filter gap x", "detect gap y", "filter gap y"))
    print("-"*100)
    for i in range(len(precision_filter)):
        print("%-4s \t %-20s \t %-20s \t %-20s \t %-20s " % (i,
            precision_detect[i][0], precision_filter[i][0],
            precision_detect[i][1], precision_filter[i][1]))
    print("-"*100)

    # 可视化(对滤波结果进行可视化)
    plt.plot(kf_filter[::, 0], kf_filter[::, 1], 'g-+')
    plt.plot(kf_predict[::, 0], kf_predict[::, 1], 'm-+')
    legend = ['reality data', 'detect data', 'filter data', 'predict data']
    plt.legend(legend, loc="best", frameon=False)
    plt.title('kalman filter')
    plt.savefig('result.svg', dpi=600)
    plt.show()

运行后输出数据如下:

卡尔曼滤波的Python实现_第2张图片

最后给出一个发射导弹跟踪敌机的应用实例如下:

# !/usr/bin/env python
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
import matplotlib.animation as animation
import sympy
import random

# 卡尔曼滤波器
class KalmanFilter:
    B = 0  # 控制变量矩阵,初始化为0
    u = 0  # 状态控制向量,初始化为0
    K = float('nan')  # 卡尔曼增益无需初始化
    z = float('nan')  # 观测值无需初始化,由外界输入
    P = np.diag(np.ones(4))  # 先验估计协方差

    x = []  # 滤波器输出状态
    G = []  # 滤波器预测状态

    # 状态转移矩阵A,和线性系统的预测机制有关
    A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)

    # 噪声协方差矩阵Q,代表对控制系统的信任程度,预测过程上叠加一个高斯噪声,若希望跟踪的轨迹更平滑,可以调小
    Q = np.diag(np.ones(4)) * 0.1

    # 观测矩阵H:z = H * x,这里的状态是(坐标x, 坐标y, 速度x, 速度y),观察值是(坐标x, 坐标y)
    H = np.eye(2, 4)

    # 观测噪声协方差矩阵R,代表对观测数据的信任程度,观测过程上存在一个高斯噪声,若观测结果中的值很准确,可以调小
    R = np.diag(np.ones(2)) * 0.1

    def init(self, px, py, vx, vy):
        # 本例中,状态x为(坐标x, 坐标y, 速度x, 速度y),观测值z为(坐标x, 坐标y)
        self.B = 0
        self.u = 0
        self.K = float('nan')
        self.z = float('nan')
        self.P = np.diag(np.ones(4))
        self.x = [px, py, vx, vy]
        self.G = [px, py, vx, vy]
        self.A = np.eye(4) + np.diag(np.ones((1, 2))[0, :], 2)
        self.Q = np.diag(np.ones(4)) * 0.1
        self.H = np.eye(2, 4)
        self.R = np.diag(np.ones(2)) * 0.1

    def update(self):
        # Xk_ = Ak*Xk-1+Bk*Uk
        a1 = np.dot(self.A, self.x)
        a2 = self.B * self.u
        x_ = np.array(a1) + np.array(a2)
        self.G = x_

        # Pk_ = Ak*Pk-1*Ak'+Q
        b1 = np.dot(self.A, self.P)
        b2 = np.dot(b1, np.transpose(self.A))
        p_ = np.array(b2) + np.array(self.Q)

        # Kk = Pk_*Hk'/(Hk*Pk_*Hk'+R)
        c1 = np.dot(p_, np.transpose(self.H))
        c2 = np.dot(self.H, p_)
        c3 = np.dot(c2, np.transpose(self.H))
        c4 = np.array(c3) + np.array(self.R)
        c5 = np.linalg.matrix_power(c4, -1)
        self.K = np.dot(c1, c5)

        # Xk = Xk_+Kk(Zk-Hk*Xk_)
        d1 = np.dot(self.H, x_)
        d2 = np.array(self.z) - np.array(d1)
        d3 = np.dot(self.K, d2)
        self.x = np.array(x_) + np.array(d3)

        # Pk = Pk_-Kk*Hk*Pk_
        e1 = np.dot(self.K, self.H)
        e2 = np.dot(e1, p_)
        self.P = np.array(p_) - np.array(e2)

    def accuracy(self, predictions, labels):
        return np.array(predictions) / np.array(labels)


# 读取敌机飞行数据
path = './9.xlsx'
label = pd.read_excel(path, header=None)
label_x = list(label.iloc[::, 0])
label_y = list(label.iloc[::, 1])
label_data = np.array(list(zip(label_x, label_y)))

# 读取我方雷达对敌机的侦查数据
path = './10.xlsx'
detect = pd.read_excel(path, header=None)
detect_x = list(detect.iloc[::, 0])
detect_y = list(detect.iloc[::, 1])
detect_data = np.array(list(zip(detect_x, detect_y)))

# 创建卡尔曼滤波器
t = len(detect_data)  # 处理时刻
kf_data_filter = np.zeros((t, 4))  # 滤波数据
kf_data_predict = np.zeros((t, 4))  # 预测数据
kf = KalmanFilter()  # 创建滤波器
kf.init(detect_x[0], detect_y[0], 0, 0)  # 滤波器初始化

# 生成地图画布
fig, ax = plt.subplots(1, 1)
plt.grid(ls='--')
ax.set_xlim(600, 800)
ax.set_ylim(300, 700)

# 初始化信息
fly_data_x = [label_data[0][0], ]
fly_data_y = [label_data[0][1], ]

missile_data_x = [625, ]
missile_data_y = [350, ]

line_fly, = plt.plot(fly_data_x[0],fly_data_y[0], 'r-')
line_missile,  = plt.plot(missile_data_x[0], missile_data_y[0], 'g-')

hit_flag = 0
hit_frame = -1
trace_flag = 1

# 计算我方导弹下一次的移动坐标
def missile_move(loc):
    global hit_flag
    solve_x = 0
    solve_y = 0
    x1, y1, x2, y2 = loc
    dist = ((x1-x2)**2 + (y1-y2)**2)**(1/2)
    max_dist = max(0.08*dist, 10)
    move_dist = min(max_dist*(0.6+random.random()), max_dist)
    if abs(dist - move_dist) < 5:
        hit_flag = 1
    x, y = sympy.symbols("x y")
    res = sympy.solve(
        [(y2-y1)*(x-x1) - (y-y1)*(x2-x1),
        ((x-x1)**2 + (y-y1)**2)**(1/2) - move_dist],
        [x, y]
        )
    for i in range(len(res)):
        if res[i][0] > min(x1, x2) and res[i][0] < max(x1, x2):
            solve_x =res[i][0]
            solve_y =res[i][1]
            break
    else:
        solve_x = x1
        solve_y = y1
    return solve_x, solve_y

# 初始化敌机、我方导弹位置
def fly_init():
    line_fly.set_data(fly_data_x, fly_data_y)
    line_missile.set_data(missile_data_x, missile_data_y)
    return line_fly, line_missile,

# 刷新敌机、我方导弹实时运动轨迹
def fly_update(frames):
    global fly_data_x, fly_data_y, missile_data_x, missile_data_y
    global line_fly, line_missile
    global hit_flag, hit_frame, trace_flag
    if hit_flag:
        hit_flag = 0
        trace_flag = 0
        hit_frame = frames.copy()
        plt.cla()
        plt.grid(ls='--')
        ax.set_xlim(600, 800)
        ax.set_ylim(300, 700)
        line_fly, = plt.plot(label_data[frames-1][0], label_data[frames-1][1], 'b*')
        line_missile, = plt.plot(label_data[frames-1][0], label_data[frames-1][1], 'b*')

    if hit_frame >= 0 and (frames >= hit_frame + 1):
        hit_frame = -1
        trace_flag = 0

    if frames >= (len(label_data) - 1):
        trace_flag = 1
        fly_data_x = [label_data[0][0], ]
        fly_data_y = [label_data[0][1], ]
        missile_data_x = [625, ]
        missile_data_y = [350, ]
        plt.cla()
        plt.grid(ls='--')
        ax.set_xlim(600, 800)
        ax.set_ylim(300, 700)
        line_fly, = plt.plot(fly_data_x[0],fly_data_y[0], 'r-')
        line_missile,  = plt.plot(missile_data_x[0], missile_data_y[0], 'g-')
    else:
        if trace_flag:
            fly_data_x.append(label_data[frames][0])
            fly_data_y.append(label_data[frames][1])
            line_fly.set_data(fly_data_x, fly_data_y)
            # ------关键处理步骤------
            kf.z = np.transpose([detect_x[frames], detect_y[frames]])  # 获取最新的观测数据
            kf.update()  # 更新卡尔曼滤波器参数
            kf_data_filter[frames, ::] = np.transpose(kf.x) # 滤波器输出
            loc = missile_data_x[frames-1], missile_data_y[frames-1],\
                    kf_data_filter[frames][0], kf_data_filter[frames][1]
            # ------关键处理步骤------
            move_x, move_y = missile_move(loc)
            missile_data_x.append(move_x)
            missile_data_y.append(move_y)
            line_missile.set_data(missile_data_x, missile_data_y)
    return line_fly, line_missile,

fly_anim = animation.FuncAnimation(fig=fig, func=fly_update,
                                frames=np.arange(1, len(label_data)),
                                init_func=fly_init, interval=100, blit=True)


plt.title('kalman filter trace object')
legend = ['fly', 'missile']
plt.legend(legend, loc="best", frameon=False)
fly_anim.save('animation.gif', writer='pillow', fps=10)
plt.show()

动画展示跟踪效果如下:

卡尔曼滤波的Python实现_第3张图片

上文仅仅是为了更好的理解卡尔曼滤波器,自己实现了相关的核心代码

若有更高的要求,filterpy模块中给出了更权威的卡尔曼滤波器,可以直接导入使用

from filterpy.kalman import KalmanFilter

卡尔曼滤波已经是一种最基本的滤波算法,结合其它算法,可以在广阔的场景下实现更强大的功能

比如作者曾经使用过的sort、deepsort等算法,其核心就是卡尔曼滤波算法

探索未知并能成功应用到预期的场景是一件有趣的事情,祝愿大家能在科研、工作上取得更多成果

你可能感兴趣的:(Python编程基础与应用,python)