专栏首页用户6953650的专栏2 用kalman滤波器估计一维匀速直线运动目标的状态
原创

2 用kalman滤波器估计一维匀速直线运动目标的状态

本例参考 参考 何友. 雷达数据处理 第二章中对于一维匀速直线运动使用kalman滤波器的例子进行仿真

说明

目标初始坐标为0,速度为5m/s,观测周期为2s,观测值标准差为1m,共观测50次。

根据上一篇的结论,kalman滤波器的R设为1,q设置5组参数,通过仿真分析进一步理解kalman滤波器的性能与参数的关系。

仿真结果

x轴坐标1.png
速度2.png
x滤波值的方差3.png
v滤波值的方差4.png
新息的统计距离5.png

结论

1 在匀速运动的状态方程中,协方差矩阵Q表示随机的加速度。

2 此值不能太小,否则稍许扰动就会导致新息的统计距离偏大,甚至超出卡方分布的置信度门限。

代码

import sys
import traceback
import getopt
import numpy as np
import matplotlib.pyplot as plt

path = 'KalmanEstimateConstantV1D/'

# 模拟观测数据
def simData(x0, v, T, sigma, N):
    truth = np.zeros(N)
    truth[0] = x0
    truth[1:] = v * T
    truth = np.cumsum(truth)  # 真实坐标
    w = np.random.normal(0, sigma, N)  # 添加观测噪声
    print('mean = ', np.mean(w))
    print('std = ', np.std(w, ddof = 1))
    data = truth + w
    # np.savetxt(path + "SimData.txt", data)  # 数据存盘
    return data, truth


# 1维数据滤波
def filter1DconstantV(z, q, r, T):
    xkk = np.zeros((2, z.size))  # 滤波值,状态向量为[x, vx]'
    pkk = np.zeros((2, 2*z.size))  # 滤波协方差,2x2矩阵
    dis = np.zeros(z.size)  # 新息统计距离

    # 状态转移矩阵
    F = [ [1, T],
           [0, 1]]
    Ft = np.transpose(F)  # F的转置
    # 协方差Q
    gama = np.array([[T**2 / 2], [T]])
    Q = q * gama @ gama.T

    # 观测矩阵
    # H = np.array([1, 0])    # H.shape is (2,) 涉及到矩阵计算,此版本矩阵转置不对,不能使用
    H = np.array([1, 0]).reshape(1, 2)    # H.shape is (1, 2)
    Ht = np.transpose(H)  # H的转置
    # 观测误差
    R = r

    # 初始化
    xkk[:, 0] = [z[0], 0]
    xkk[:, 1] = [z[1], (z[1] - z[0])/T]
    pkk[:, 2:4] = [[r, r/T],
                   [r/T, 2*r / T**2]]

    # 依次滤波
    for i in range(2, z.size):
        xki = F @ xkk[:, i-1]  # 状态的一步预测
        pki = F @ pkk[:, 2*(i-1):2*i] @ Ft + Q  # 协方差的一步预测
        zki = H @ xki  # 量测的预测
        S = H @ pki @ Ht + R  # 新息的协方差,此例中S为标量
        v = z[i] - zki  # 新息
        if abs(S) < 1e-3 :  # S不可逆
            print('|S| < 1e-3')
            K = 0  # 增益
            dis[i] = -1  # 新息统计距离,异常值
        else:
            invS = 1 / S
            K = pki @ Ht * invS  # 增益
            dis[i] = v * invS * v  # 新息统计距离

        xkk[:, i] = (xki.reshape(2, 1) + K * v).reshape(2,)  # 状态的更新
        pkk[:, 2*i : 2*(i+1)] = pki - S * K @ np.transpose(K)  # 协方差的更新
        # pkk[:, 2*i : 2*(i+1)] = (np.eye(2) - K @ H) @ pki  # 协方差的更新
        if np.min(np.linalg.eigvals(pkk[:, 2*i : 2*(i+1)])) < 1e-3 :
            print(f'i = {i}, pkk不正定')

    return xkk, pkk, dis

# 画图
# 输入:
#       truth, 真值
#       z, 观测值
#       T,采样间隔
#       rs, 不同参数组R
#       qs, 不同参数组Q
#       xkk, 各组参数对应的滤波值
#       pkk, 各组参数对应的滤波协方差矩阵
#       dis,各组参数对应的统计距离
#       saveFig,是否保存图片
def display(truth, z, T, rs, qs, xkk, pkk, dis, saveFig):
    N = truth.size

    # 颜色表
    colors = ('b', 'g', 'r', 'y', 'c',  # 参数1 - 5
              'm',  # 观测值用
              'orange')  # 真值用

    # markers
    markers = ('o', 'v', 'd', 'x', '+')  # 参数1 - 5

    # 画 真值、观测值、滤波值
    title = 'x轴坐标1'
    plt.figure(title)
    plt.plot(np.arange(N) + 1, truth, color = colors[len(colors) - 1], label = '真值')  # plot
    plt.plot(np.arange(N) + 1, z, '.-', color = colors[len(colors) - 2], label = '观测值')  # plot
    for row in range(0, len(rs)):
        plt.plot(np.arange(N) + 1, xkk[2*row, :], 'o-',
                 marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

    plt.legend()
    plt.xlabel('t(s)')  # 设置x轴标签
    plt.ylabel('x(m)')  # 设置y轴标签
    plt.grid()
    if saveFig:
        plt.savefig(path + title)

    # 画速度
    title = '速度2'
    plt.figure(title)
    plt.plot(np.arange(N-1) + 1, np.diff(truth[:2]) * np.ones(N-1) / T, color = colors[len(colors) - 1], label = '真值')  # plot
    plt.plot(np.arange(N-1) + 1, np.diff(z) * (1/T), '.-', color = colors[len(colors) - 2], label = '观测值')  # plot
    for row in range(0, len(rs)):
        plt.plot(np.arange(N-1) + 1, xkk[2*row+1, 1:], 'o-',
                 marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

    plt.legend()
    plt.xlabel('t(s)')  # 设置x轴标签
    plt.ylabel('v(m/s)')  # 设置y轴标签
    plt.grid()
    if saveFig:
        plt.savefig(path + title)

    # 画 滤波值的方差
    title = 'x滤波值的方差3'
    fig = plt.figure(title)
    for row in range(0, len(rs)):
        plt.plot(np.arange(N-1) + 1, pkk[2*row, 2::2], 'o-',
                 marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

    plt.legend()
    plt.xlabel('t(s)')  # 设置x轴标签
    plt.ylabel('pkk_x')  # 设置y轴标签
    plt.grid()
    if saveFig:
        plt.savefig(path + title)

    # 画 滤波值的方差
    title = 'v滤波值的方差4'
    fig = plt.figure(title)
    for row in range(0, len(rs)):
        plt.plot(np.arange(N-1) + 1, pkk[2*row+1, 3::2], 'o-',
                 marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

    plt.legend()
    plt.xlabel('t(s)')  # 设置x轴标签
    plt.ylabel('pkk_v')  # 设置y轴标签
    plt.grid()
    if saveFig:
        plt.savefig(path + title)

    # 画 新息的统计距离
    title = '新息的统计距离5'
    fig = plt.figure(title)
    for row in range(0, len(rs)):
        plt.plot(np.arange(N-2) + 1, dis[row, 2:], 'o-',
                 marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

    plt.legend()
    plt.xlabel('t(s)')  # 设置x轴标签
    plt.ylabel('统计距离')  # 设置y轴标签
    plt.grid()
    if saveFig:
        plt.savefig(path + title)

    plt.show()

# Works 实现主要功能
def Works():
    saveFig = True  # 保存图片
    # saveFig = False  # 不保存图片

    x0 = 0  # 初始值
    v = 5  # 速度,单位:1m/s
    T = 2  # 观测周期,单位:1s
    r = 1e-0  # 观测值标准差,单位:1m
    N = 50  # 观测次数
    z, truth = simData(x0, v, T, r, N)  # 模拟观测数据
    # plt.figure("模拟数据")
    # plt.plot(z, '.-')

    q = 8 * (r**2) / (T**4)  # 根据两点间匀加速运动推导得到加速度的标称值
    # 只能说q不超过该标称值,但不能将q设为该值
    # 对于匀速运动,q表示加速度,应充分小,符合状态方程是依据匀速运动变化的假设
    q *= 0.01

    # 五组参数
    rs = [r,    r,     r,      r,    r]
    qs = [q/4,  q/2,   q,    2*q,  4*q]
    xkk = np.zeros((2*len(rs), z.size))  # 滤波值,每2行依次对应一组参数
    pkk = np.zeros((2*len(rs), 2*z.size))  # 滤波协方差,每2行对应一组参数,每2列对应一次滤波
    dis = np.zeros((len(rs), z.size))  # 新息统计距离,各行依次对应一组参数

    for idxR in range(0, len(rs)):
        r = rs[idxR]
        q = qs[idxR]
        xkk[2*idxR:(2*idxR+2), :], pkk[2*idxR:(2*idxR+2), :], dis[idxR, :] = filter1DconstantV(z, q, r, T)  # 滤波

    # 画图
    display(truth, z, T, rs, qs, xkk, pkk, dis, saveFig)

    return 0

# 异常处理函数
class Usage(Exception):
    def __init__(self, msg):
        self.msg = msg


# main()函数
def main(argv=None):
    if argv is None:
        argv = sys.argv
    try:
        try:
            opts, args = getopt.getopt(argv[1:], "h", ["help"])
        except(getopt.error, msg):
            raise (Usage(msg))

        # 使plt支持中文
        plt.rcParams['font.sans-serif'] = ['SimHei']
        plt.rcParams['axes.unicode_minus'] = False

        # Works 实现主要功能
        Works()
    except Usage as err:  # 待处理新增代码中的异常
        print(sys.stderr, err.msg)
        print(sys.stderr, "for help use --help")
        return 1
    except Exception as e:
        print("print_exception()")
        exc_type, exc_value, exc_tb = sys.exc_info()
        print('the exc type is:', exc_type)
        print('the exc value is:', exc_value)
        print('the exc tb is:', exc_tb)
        traceback.print_exception(exc_type, exc_value, exc_tb)
    finally:
        print('exit main')


# 直接运行的入口
if "__main__" == __name__:
    sys.exit(main())

原创声明,本文系作者授权云+社区发表,未经许可,不得转载。

如有侵权,请联系 yunjia_community@tencent.com 删除。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 1,Kalman滤波器参数如何选取

    新冠居家封闭期间,对参考文献中估计常数的例子,初次使用python的NumPy库进行仿真,深入理解Kalman滤波器的参数对滤波性能的影响。

    雷科
  • matplotlib 中文支持

    雷科
  • numpy中关于向量的坑

    雷科
  • Python可视化学习(饼状图,坐标系...)

    今天资源君带大家学习一下Python的可视化,何谓可视化呢?我们常常听说Python的数据分析,数据分析中很重要的一个就是将数据展示出来,如何展示出来呢...

    Python进击者
  • 深度学习数学基础一--最小二乘法

    之前总是先上手一些比较高级的神经网络算法,CNN,RNN等。可是总觉得有些知识原理总是羁绊着我进一步理解。这才意识到基础的重要性。所以,就一点一点的从基础数学最...

    zenRRan
  • matplot 同时绘制多个图形(一)

    matplotlib.pyplot中的subplot()函数可以用来在一张画布上绘制多个图形。

    用户6021899
  • python画箱线图

    py3study
  • 零基础学编程042:画函数图像

    孩子马上就要参加高考了,我以前还能帮着辅导一下数学功课,现在就不行了,一来她很忙,晚上很晚才到家,二来高中的数学题太变态,琢磨一个小时可能也解不出一道。 前几天...

    申龙斌
  • Caffe2 - (八)图像加载与预处理

    Caffe 使用的是 OpenCV 的 Blue-Green-Red (BGR),而不是通用的 Red-Green-Blue (RGB).

    AIHGF
  • Matplotlib新手上路(中)

    接上回继续 一、多张图布局(subplot) 1.1 subplot布局方式 import matplotlib.pyplot as plt plt.figu...

    菩提树下的杨过

扫码关注云+社区

领取腾讯云代金券