$$ \left\{\begin{array}{l}\boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=h\left(\boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k}\end{array} \quad k=1, \ldots, N\right. $$

$$ P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right) $$

$$ \begin{array}{c} P\left(\boldsymbol{x}_{k},z_k \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) =P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right)·P(z_k)\\ = P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) \end{array} $$

$$ P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k}\right) \propto P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) $$

$$ P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=\int P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1} $$

$$ P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right) $$

$$ P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=P\left(\boldsymbol{x}_{k-1} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k-1}, \boldsymbol{z}_{1: k-1}\right) $$

$$ \left\{\begin{array}{l}\boldsymbol{x}_{k}=\boldsymbol{A}_{k} \boldsymbol{x}_{k-1}+\boldsymbol{u}_{k}+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k}=\boldsymbol{C}_{k} \boldsymbol{x}_{k}+\boldsymbol{v}_{k}\end{array} \quad k=1, \ldots, N\right. $$

$$ \boldsymbol{w}_{k} \sim N(\mathbf{0}, \boldsymbol{R}) . \quad \boldsymbol{v}_{k} \sim N(\mathbf{0}, \boldsymbol{Q}) $$

$$ \begin{aligned} \operatorname{Cov}(x) & =\Sigma \\ \operatorname{Cov}(\mathbf{A} x) & =\mathbf{A} \Sigma \mathbf{A}^{T}\end{aligned} $$

$$ P\left(\boldsymbol{x}_{k} \mid \boldsymbol{x}_{0}, \boldsymbol{u}_{1: k}, \boldsymbol{z}_{1: k-1}\right)=N\left(\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R}\right) $$

$$ \check{\boldsymbol{x}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \quad \check{\boldsymbol{P}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{\mathrm{T}}+\boldsymbol{R} $$

$$ P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right)=N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}\right) $$

$$ \boldsymbol{x}_{k} \sim N\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right) $$

$$ N\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)=\eta N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}\right) \cdot N\left(\check{\boldsymbol{x}}_{k}, \check{\boldsymbol{P}}_{k}\right) $$

$$ \left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)^{\mathrm{T}} \hat{\boldsymbol{P}}_{k}^{-1}\left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)=\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{x}_{k}\right)^{\mathrm{T}} \boldsymbol{Q}^{-1}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{x}_{k}\right)+\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right)^{\mathrm{T}} \check{\boldsymbol{P}}_{k}^{-1}\left(\boldsymbol{x}_{k}-\check{\boldsymbol{x}}_{k}\right) $$

$$ \hat{\boldsymbol{P}}_{k}^{-1}=\boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k}+\check{\boldsymbol{P}}_{k}^{-1} $$

$$ \boldsymbol{K}=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} $$

$$ \boldsymbol{I}=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k}+\hat{\boldsymbol{P}}_{k} \check{\boldsymbol{P}}_{k}^{-1}=\boldsymbol{K} \boldsymbol{C}_{k}+\hat{\boldsymbol{P}}_{k} \check{\boldsymbol{P}}_{k}^{-1} $$

$$ \hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \check{\boldsymbol{P}}_{k} $$

$$ \begin{aligned} -2 \hat{\boldsymbol{x}}_{k}^{\mathrm{T}} \hat{\boldsymbol{P}}_{k}^{-1} \boldsymbol{x}_{k}&=-2 \boldsymbol{z}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k} \boldsymbol{x}_{k}-2 \check{\boldsymbol{x}}_{k}^{\mathrm{T}} \check{\boldsymbol{P}}_{k}^{-1} \boldsymbol{x}_{k} \\ \hat{\boldsymbol{P}}_{k}^{-1} \hat{\boldsymbol{x}}_{k}&=\boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{z}_{k}+\check{\boldsymbol{P}}_{k}^{-1} \check{\boldsymbol{x}}_{k} \\ \hat{\boldsymbol{x}}_{k} & =\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{\mathrm{T}} \boldsymbol{Q}^{-1} \boldsymbol{z}_{k}+\hat{\boldsymbol{P}}_{k} \check{\boldsymbol{P}}_{k}^{-1} \check{\boldsymbol{x}}_{k} \\ \hat{\boldsymbol{x}}_{k}&=\boldsymbol{K} \boldsymbol{z}_{k}+\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right)\check{\boldsymbol{x}}_{k} \\ \hat{\boldsymbol{x}}_{k}&=\check{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \check{\boldsymbol{x}}_{k}\right)\end{aligned} $$

$$ \begin{array}{l}\hat{\boldsymbol{x}}_{k}=\check{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \check{\boldsymbol{x}}_{k}\right) \\ \hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \check{\boldsymbol{P}}_{k}\end{array} $$

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import cv2
def target_simple(t):
    x_t = 100
    y_t = 100 + t
    return x_t, y_t
def get_u(target_xy, cur_xy, length):
    d_v = np.mat(target_xy).T - cur_xy
    mode_d = (np.array(d_v) ** 2).sum() ** 0.5
    u = d_v / mode_d * length
    return u
d_t = 1
# 初始数据 x y v_x v_y
data_init = np.mat([[0], [0], [0], [0]])
# 初始协方差矩阵
P_init = np.zeros([4, 4])
# 状态转移矩阵
A = np.mat([
    [1, 0, d_t, 0],
    [0, 1, 0, d_t],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
])
# 控制输出矩阵
B = np.mat([
    [0.5 * d_t * d_t, 0],
    [0, 0.5 * d_t * d_t],
    [d_t, 0],
    [0, d_t]
])
# 控制噪声协方差矩阵
sigma_r = 0.0
R = np.mat(np.eye(4) * (1 - sigma_r) + sigma_r) * 0.1
# 观测状态转移矩阵
C = np.eye(4)
# 观测噪声协方差矩阵
sigma_q = 0.0
Q = np.mat(np.eye(4) * (1 - sigma_q) + sigma_q) * 0.1
# 加速度大小
u_l = 1
data_list = [data_init]
P_list = [P_init]
target_pos_list = [target_simple(0)]
# 生成地图画布
fig, ax = plt.subplots(1, 1)
plt.grid(ls='--')
ax.set_xlim(-100, 500)
ax.set_ylim(-100, 800)
line_missile, = plt.plot(data_list[0][0],data_list[0][1], 'r-')
line_fly, = plt.plot(target_pos_list[0][0],target_pos_list[0][1], 'b-')
label_data = 200
def fly_update(index):
    x_t, y_t = target_simple(index)
    target_pos_list.append(target_simple(index))
        cur_u = get_u([x_t, y_t], data_list[index - 1][:2], u_l)
        x_pre = A * data_list[index - 1] + B * cur_u
    p_pre = A * P_list[index - 1] *  A.T + R
        control_noise = np.mat(np.random.multivariate_normal([0, 0, 0, 0], R, 1).T)
    real_x = x_pre + control_noise
        view_noise = np.mat(np.random.multivariate_normal([0, 0, 0, 0], Q, 1).T)
    z_k = C * real_x + view_noise
        K = (p_pre * C) * ((C * p_pre * C.T + Q) ** -1)
        x_post = x_pre + K * (z_k - C * x_pre)
    P_post = (np.mat(np.eye(4))- K * C) * p_pre
        data_list.append(x_post)
    P_list.append(P_post)
    pos_data = np.array(data_list)[:, :2, 0]
    fly_data = np.array(target_pos_list)[:, :2]
    line_missile, = plt.plot(pos_data[:, 0],pos_data[:, 1], 'r-')
    line_fly, = plt.plot(fly_data[:, 0],fly_data[:, 1], 'b-')
    return line_missile, line_fly,
def fly_init():
    pos_data = np.array(data_list)[:, :2, 0]
    line_missile.set_data(pos_data[:, 0], pos_data[:, 1])
    fly_data = np.array(target_pos_list)[:, :2]
    line_fly, = plt.plot(fly_data[:, 0],fly_data[:, 1], 'b-')
    return line_missile, line_fly,
fly_anim = animation.FuncAnimation(fig=fig, func=fly_update,
                                frames=np.arange(1, label_data),
                                init_func=fly_init, interval=100, blit=True)
fly_anim.save('vvd_missile.gif', writer='pillow', fps=10)
plt.show()