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

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