线性滤波与非线性滤波实验

发表于 更新于 分类于 阅读次数： Waline： 本文字数： 16k 阅读时长 ≈ 14 分钟

本文围绕自由落体运动的估计，进行了线性滤波和非线性滤波的实验。下面这张图是源自西安交通大学蔡远利教授的《随即滤波与控制》课程。 该课程主要围绕估计，平滑与预测三方面讲解各类滤波方法。

Map of Control Theory

问题描述

  1. 已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表：

    image-20220617114332337

  2. 同样考虑自由落体运动的物体，用雷达（和物体在同一水平面）进行测量，如图所示，如果雷达测距和测角的量测噪声为高斯白噪声序列，均值为0，方差阵 \(R=\begin{bmatrix}0.04 &0\\0&0.01\end{bmatrix}\)，试根据以下测量数据确定物体的高度和下落速度随时间变化的估计值：

    该下落运动和雷达检测信息如下图所示：

模型介绍

1. 线性估计问题

设在自由落体运动时，物体高度为 \(h\)，速度为 \(v\) （向下为正），设 \(x_1=h,x_2=v\)，则系统的状态方程为：

\[ \begin{bmatrix} \dot x_1\\\dot x_2 \end{bmatrix}= \begin{bmatrix} 0&-1\\ 0&0\\ \end{bmatrix} \begin{bmatrix} x_1\\x_2 \end{bmatrix}+ \begin{bmatrix} 0\\1 \end{bmatrix}g\ \ (g=9.80m/s^2) \]

由上述状态方程可以解出系统的状态转移函数为：

\[ x(t)=\begin{bmatrix} 1&-t\\ 0&1 \end{bmatrix}x(0) +\begin{bmatrix} -t\\1 \end{bmatrix}u(t) \]

当步长为 1s 时系统的状态转移函数为：

\[ \Phi_{k+1,k}=\begin{bmatrix} 1&-1\\ 0&1 \end{bmatrix} \ \Psi_{k+1,k}=\begin{bmatrix} -1\\ 1 \end{bmatrix} \]

由此可得，系统的一步预测为：

\[ x_{k+1,k}=\Phi_{k+1,k}x_{k|k}+\Psi_{k+1,k}u_k \]

由于观测是对物体高度的直接测量，并且假设量测误差为均值为0，方差为1的白噪声，则量测方程为： \[ y_k=H_kx_k+v_k \]

其中： \[ H_k\equiv \begin{bmatrix} 1\\0 \end{bmatrix}, \ v_k \sim N(0,1), \ R=1 \]

本题给出的数据中，量测高度单位为\(km\)，加速度为\(9.8m/s\)，为便于计算，统一距离单位为\(km\)，时间单位统一为\(s\)，即重力加速度\(g=0.0098km/s\)。由于滤波初值未给定，根据量测数据估算初始高度为\(2km\)，初始速度为0，即\(x_{0|0}=\begin{bmatrix}2&0\end{bmatrix}\)。由于这样确定的初始估计值比较粗糙，因此方差较大，设定系统初始方差阵为\(P_{0|0}=\begin{bmatrix}10&0\\0&10\end{bmatrix}\)。

建立卡尔曼滤波： \[ x_{k+1,k}=\Phi_{k+1,k}x_{k|k}+\Psi_{k+1,k}u_k\\ P_{k+1|k}=\Phi_{k+1,k}P_{k|k}\Phi^\top_{k+1,k}\\ P_{k+1|k+1}=[P_{k+1|k}^{-1}+H_{k+1}^{\top} R_{k+1}^{-1}H_{k+1}]^{-1}\\ K_{k+1}=P_{k+1|k+1}H_{k+1}^\top R_{k+1}^{-1}\\ \hat x_{k+1|k+1}=\hat x_{k+1|k}+K_{k+1}(y_{k+1}-H_{k+1}\hat x_{k+1|k}) \] 对物体自由落体下降高度和速度进行计算，计算结果见后文实验结果部分。

2. 非线性估计问题

在第2个问题中，量测与估计值为非线性关系，考虑采用非线性卡尔曼滤波算法对其进行估计，本报告中采用扩展式卡尔曼滤波算法（EKF）。

设测量的斜距为\(l(km)\)，俯仰角为\(\theta(rad)\)，物体高度为\(h(km)\)，下降速度为\(v(km/s)\)（向下为正），物体在地面上的竖直投影到雷达的距离为\(d_0\)。

为便于后续线性化计算，选取系统状态量为： \[ x_1=h,\ x_2=v,\ x_3=d_0 \] 则系统的状态方程为： \[ \dot x= \begin{bmatrix} 0&-1&0\\ 0&0&0\\ 0&0&0 \end{bmatrix}x+ \begin{bmatrix} 0\\ 1\\ 0 \end{bmatrix}g,\ 其中\ g=0.0098\ km/s \] 求解得出当时间间隔为\(0.5s\)时，系统的离散状态转移函数为： \[ \Phi_{k+1,k}=\begin{bmatrix} 1&-0.5&0\\ 0&1&0\\ 0&0&1 \end{bmatrix},\ \Psi_{k+1,k}=\begin{bmatrix} -0.125\\ 0.5\\ 0 \end{bmatrix} \] 系统观测为雷达测量出的斜距和俯仰角，则： \[ y_1=l=\sqrt{h^2+d_0^2}=\sqrt{x_1^2+x_3^2}\\ y_2=\theta=\arctan(\frac{h}{d_0})=\arctan(\frac{x_1}{x_3}) \] 观测量对状态量求导可得： \[ \frac{\partial y_1}{\partial x_1}=\frac{x_1}{\sqrt{x_1^2+x_3^2}}\\ \frac{\partial y_1}{\partial x_2}=0\\ \frac{\partial y_1}{\partial x_3}=\frac{x_3}{\sqrt{x_1^2+x_3^2}}\\ \frac{\partial y_2}{\partial x_1}=\frac{x_3}{x_1^2+x_3^2}\\ \frac{\partial y_2}{\partial x_2}=0\\ \frac{\partial y_2}{\partial x_3}=-\frac{x_1}{x_1^2+x_3^2}\\ \] 因此，系统的局部线性化系数为： \[ H=\begin{bmatrix} \frac{x_1}{\sqrt{x_1^2+x_3^2}}&0&\frac{x_3}{\sqrt{x_1^2+x_3^2}}\\ \frac{x_3}{x_1^2+x_3^2}&0&-\frac{x_1}{x_1^2+x_3^2} \end{bmatrix} \] 扩展卡尔曼滤波（EKF）的计算过程如下： \[ x_{k+1,k}=\Phi_{k+1,k}x_{k|k}+\Psi_{k+1,k}u_k\\ P_{k+1|k}=\Phi_{k+1,k}P_{k|k}\Phi^\top_{k+1,k}\\ P_{k+1|k+1}=[P_{k+1|k}^{-1}+H_{k+1}^{\top} R_{k+1}^{-1}H_{k+1}]^{-1}\\ K_{k+1}=P_{k+1|k+1}H_{k+1}^\top R_{k+1}^{-1}\\ \hat x_{k+1|k+1}=\hat x_{k+1|k}+K_{k+1}(y_{k+1}-H_{k+1}\hat x_{k+1|k}) \] 其中，与卡尔曼滤波不同的地方在于\(H\)系数矩阵是通过在观测点进行局部线性化得到的。通过上述步骤即可对非线性观测进行滤波估计。

实验结果

1. 线性滤波问题

根据题中给出的高度观测数据进行线性卡尔曼滤波，计算得到的结果如下：

高度估计数据为：

时间 1 2 3 4 5 6 7
高度 1.9943 1.9791 1.9550 1.9211 1.8774 1.8244 1.7605
时间 8 9 10 11 12 13 14
高度 1.6870 1.6038 1.5103 1.4075 1.2947 1.1723 1.0400
时间 15 16 17 18 19 20
高度 0.8980 0.7460 0.5845 0.4129 0.2317 0.0405

绘制出高度变化图如下：

下落速度（向下为正）滤波数据如下表所示：

时间 1 2 3 4 5 6 7
下落速度 0.0078 0.0157 0.0247 0.0343 0.0439 0.0536 0.0635
时间 8 9 10 11 12 13 14
下落速度 0.0733 0.0831 0.0930 0.1028 0.1126 0.1224 0.1322
时间 15 16 17 18 19 20
下落速度 0.1420 0.1519 0.1616 0.1714 0.1812 0.1911

绘制出速度变化图如下：

同时，高度和下落速度的估计方差随时间变化为：

高度估计方差
速度估计方差

从上图中的结果可以看出，卡尔曼滤波对于物体下落高度和速度的估计在新息一开始加入后就迅速减小，之后渐进减小到0附近之后不再有明显下降。物体自由落体的高度变化曲线为抛物线，速度为匀加速变化，符合自由落体的相关物理定律。

2. 非线性估计问题

根据题中给出的量测数据以及上文中提到的非线性滤波模型，滤波估计出下落物体的高度和速度变化如下。

物体高度变化为：

物体下落速度变化为：

物体自由下落高度和速度估计误差为：

高度估计方差
速度估计方差

从上图估计结果可以看出，下落高度随时间呈抛物线变化，下落速度（向下为正）随时间匀速上升，符合自由落体物理规律。

实验过程中的一些发现

在最开始的编程和实验过程中，由于题目给出的量测数据以\(km\)为单位，为便于计算，将距离单位统一换算为\(km\)处理。但是由于题面中未给出量测方差的单位，故未对该数值进行换算，即按照单位为\(km\)进行处理。

在求解第1题过程中，未发现任何异常，然而在求解第二题时，如果未对量测协方差进行单位换算，将其单位当作\(km\)进行处理，估计出的结果会如下图所示：

高度变化
速度变化

对比上图中计算结果和实验结果中第二问的计算结果可以看出，相对于线性卡尔曼滤波，非线性卡尔曼滤波在计算时收到量测误差的影响更大。这是由于非线性滤波时量测系数矩阵\(H\)是经过局部线性化得到的，如果量测误差本身较大，则会影响卡尔曼系数的计算结果，最终导致估计结果不准确。

附录

常量配置

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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
'''
@ Author: Yu Zhao
@ Contact: yuzhaokz@163.com
@ Date: 2022-06-10 16:59:11
@ LastEditors: Yu Zhao
@ LastEditTime: 2022-06-17 21:02:09
@ Description: 知识因被记录而产生价值
@ Website: https://kezhi.tech
'''

import numpy as np

cache_dir="cache/"

# observation

observation_1=[
    [1.9945],
    [1.9794],
    [1.9554],
    [1.9214],
    [1.8777],
    [1.8250],
    [1.7598],
    [1.6867],
    [1.6036],
    [1.5092],
    [1.4076],
    [1.2944],
    [1.1724],
    [1.0399],
    [0.8980],
    [0.7455],
    [0.5850],
    [0.4125],
    [0.2318],
    [0.0399],
]

observation_2=[
    [2.82741643781891,0.00075850435876],
    [2.82519811729771,0.00083282260478],
    [2.82066686966236,0.00067808241639],
    [2.81487233105901,0.00085279036802],
    [2.80671786536244,0.00072900768452],
    [2.79725268974089,0.00080072481819],
    [2.78664273475039,0.00075095576213],
    [2.77320365026313,0.00065762725379],
    [2.75919535464551,0.00081186148545],
    [2.74331288628195,0.00079783727034],
    [2.72538888482812,0.00073060712986],
    [2.70664967712312,0.00063242006530],
    [2.68632403406473,0.00063656524495],
    [2.66386533852220,0.00080659845639],
    [2.64093529707333,0.00067704740069],
    [2.61621111727357,0.00076573767706],
    [2.59038109850785,0.00054955759081],
    [2.56298794272843,0.00058487913971],
    [2.53498317950797,0.00055602747368],
    [2.50647589372246,0.00033550412588],
    [2.47571075016386,0.00056012688452],
    [2.44560676000982,0.00056694491978],
    [2.41403690772088,0.00059380631025],
    [2.38252228611696,0.00053681916544],
    [2.35016501182332,0.00065871960781],
    [2.31790939837137,0.00068598344328],
    [2.28597616656453,0.00060922471348],
    [2.25418431681401,0.00057086018918],
    [2.22259320219535,0.00041308535708],
    [2.19237398969466,0.00047302026281],
    [2.16290177997271,0.00030949309972],
    [2.13441725793706,0.00040552624986],
    [2.10811064690727,0.00037545033142],
    [2.08322179823195,0.00017282319262],
    [2.06148109026767,0.00020758327980],
    [2.04219885094031,0.00037186464579],
    [2.02610235314357,0.00018082163465],
    [2.01290326863579,0.00023323830160],
    [2.00463157388395,-0.00004536186964],
    [2.00058143251913,0.00003246284068],
]

# dimension transform
observation_2=[[observation[0],observation[1]*1000] for observation in observation_2]


# the parameters of problem 1
param_1={
    
    "Phi":np.matrix(
        [
            [1,-1],
            [0, 1]
        ]
    ),
    
    "Psi":np.matrix(
        [
            [-1],
            [1],
        ]
    ),
    
    "H":np.matrix(
        [
            [1,0],
        ]
    ),
    
    "R":np.matrix(
        [1/1000]
    ),
}

# the parameters of problem 2
param_2={
    
    "Phi":np.matrix(
        [
            [1,-0.5,0],
            [0,1,0],
            [0,0,1],
        ]
    ),
    
    "Psi":np.matrix(
        [
            [-0.125],
            [0.5],
            [0],
        ]
    ),
    
    "R":np.matrix(
        [
            [0.04/1000,0],
            [0,0.01],
        ]
    )
}

题目1 代码

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
118
119
120
121
122
123
124
125
126
127
128
129
'''
@ Author: Yu Zhao
@ Contact: yuzhaokz@163.com
@ Date: 2022-06-10 14:44:08
@ LastEditors: Yu Zhao
@ LastEditTime: 2022-06-17 12:45:17
@ Description: 知识因被记录而产生价值
@ Website: https://kezhi.tech
'''

import os
from pprint import pprint
from matplotlib import pyplot as plt
import numpy as np

from config import cache_dir,observation_1,param_1

# 支持中文
plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号

# 创建cache文件夹
if not os.path.exists(cache_dir):
    os.mkdir(cache_dir)


# Kalman filter
def Kalman_filter(x_0:np.matrix,  # the status of last step
                  P_0:np.matrix,  # the variance of last step
                  y_1:np.matrix,  # the observation of this step
                  u_0:np.matrix=None, # the input of system
                  Phi:np.matrix=None,  # the status transform matrix 
                  Psi:np.matrix=None,  # coefficient of system input
                  Gamma:np.matrix=None,   # coefficient of observing noise
                  H:np.matrix=None,    # y=Hx+v
                  R:np.matrix=None,    # the variance of observing noise
                  Q:np.matrix=None,    # the variance of status noise
):
    # variable check
    if u_0 is None:
        u_0=np.matrix(0)
        print("u_0 is lacked, use default value: {}".format(u_0))
        
    if Phi is None:
        Phi=np.matrix(np.eye(x_0.shape[0]))
        print("Phi is lacked, use default value: {}".format(Phi))
    
    if Psi is None:
        Psi=np.matrix(np.zeros((x_0.shape[0],1)))
        print("Psi is lacked, use default value: {}".format(Psi))
    
    if Gamma is None:
        Gamma=Psi.copy()
        print("Gamma is lacked, use default value: {}".format(Gamma))
        
    if H is None:
        H=np.matrix(np.ones((y_1.shape[0],x_0.shape[0])))
        print("H is lacked, use default value: {}".format(H))
        
    if R is None:
        R=np.matrix(np.zeros((y_1.shape[0],1)))
        print("R is lacked, use default value: {}".format(R))
        
    if Q is None:
        Q=np.matrix(0)
        print("Q is lacked, use default value: {}".format(Q))
        
    # one step prediction
    x_10=Phi@x_0+Psi@u_0
    P_10=Phi@P_0@Phi.T+Gamma@Q@Gamma.T
    # time amend
    P_11=(P_10.I+H.T@R.I@H).I
    K_1=P_11@H.T@R.I
    x_11=x_10+K_1@(y_1-H@x_10)
    return x_11,P_11,K_1


def show_result(record:dict,problem):
    pprint(record)
    for key,value in record.items():
        plt.title(key)
        plt.xlabel("step")
        if key=="h":
            plt.ylabel("高度:km")
        if key=="v":
            plt.ylabel("速度:km/s (向下为正)")
        plt.plot(value)
        plt.savefig(os.path.join(cache_dir,problem+"_"+key+".png?x-oss-process=style/webp"),dpi=600)
        plt.clf()
        plt.cla()
    

if __name__=="__main__":
    
    # solve problem 1
    x_0=np.matrix([[2],[0]])
    P_0=np.matrix([[10,0],[0,10]])
    x_record=[x_0]
    P_record=[P_0]
    K_record=[None]
    for obs in observation_1:
        y_1=np.matrix(obs).T
        x_1,P_1,K_1=Kalman_filter(
            x_0,
            P_0,
            y_1,
            u_0=np.matrix(9.8/1000),
            Phi=np.matrix(param_1["Phi"]),
            Psi=np.matrix(param_1["Psi"]),
            H=np.matrix(param_1["H"]),
            R=np.matrix(param_1["R"]),
        )
        
        x_record.append(x_1)
        P_record.append(P_1)
        K_record.append(K_1)
        
        x_0=x_1
        P_0=P_1
        
    result_record={
        "h":[x[0,0] for x in x_record],
        "v":[x[1,0] for x in x_record],
        "P_0":[P[0,0] for P in P_record],
        "P_1":[P[1,1] for P in P_record],
        # "K":K_record,
    }
    show_result(result_record,problem="p1")

题目2代码

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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
'''
@ Author: Yu Zhao
@ Contact: yuzhaokz@163.com
@ Date: 2022-06-17 16:54:31
@ LastEditors: Yu Zhao
@ LastEditTime: 2022-06-17 21:02:28
@ Description: 知识因被记录而产生价值
@ Website: https://kezhi.tech
'''

import os
from pprint import pprint
from matplotlib import pyplot as plt
import numpy as np

from config import cache_dir,observation_2,param_2

# 支持中文
plt.rcParams['font.sans-serif'] = ['SimHei']  # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False  # 用来正常显示负号

# 创建cache文件夹
if not os.path.exists(cache_dir):
    os.mkdir(cache_dir)

# compute H in EKF
def get_H(x):
    l_2=x[0,0]**2+x[2,0]**2
    return np.matrix(
        [
            [x[0,0]/np.sqrt(l_2),0,x[2,0]/np.sqrt(l_2)],
            [x[2,0]/l_2,0,-x[0,0]/l_2],
        ]
    )

# function of observation
def y(x):
    return np.matrix(
        [
            [np.sqrt(x[0,0]**2+x[2,0]**2)],
            [np.arctan(x[0,0]/x[2,0])],
        ]
    )
    

# Kalman filter
def EKF(x_0:np.matrix,  # the status of last step
        P_0:np.matrix,  # the variance of last step
        y_1:np.matrix,  # the observation of this step
        u_0:np.matrix=None, # the input of system
        Phi:np.matrix=None,  # the status transform matrix 
        Psi:np.matrix=None,  # coefficient of system input
        Gamma:np.matrix=None,   # coefficient of observing noise
        R:np.matrix=None,    # the variance of observing noise
        Q:np.matrix=None,    # the variance of status noise
):
    # variable check
    if u_0 is None:
        u_0=np.matrix(0)
        print("u_0 is lacked, use default value: {}".format(u_0))
        
    if Phi is None:
        Phi=np.matrix(np.eye(x_0.shape[0]))
        print("Phi is lacked, use default value: {}".format(Phi))
    
    if Psi is None:
        Psi=np.matrix(np.zeros((x_0.shape[0],1)))
        print("Psi is lacked, use default value: {}".format(Psi))
    
    if Gamma is None:
        Gamma=np.matrix(np.zeros((x_0.shape[0],1)))
        print("Gamma is lacked, use default value: {}".format(Gamma))
        
    if R is None:
        R=np.matrix(np.zeros((y_1.shape[0],1)))
        print("R is lacked, use default value: {}".format(R))
        
    if Q is None:
        Q=np.matrix(0)
        print("Q is lacked, use default value: {}".format(Q))
        
    # one step prediction
    x_10=Phi@x_0+Psi@u_0
    P_10=Phi@P_0@Phi.T+Gamma@Q@Gamma.T
    # local linearization
    H=get_H(x_10)
    # time amend
    P_11=(P_10.I+H.T@R.I@H).I
    K_1=P_11@H.T@R.I
    x_11=x_10+K_1@(y_1-y(x_10))
    return x_11,P_11,K_1

def mse(x_1:np.matrix,x_2:np.matrix):
    loss=0
    for i in range(x_1.shape[0]):
        for j in range(x_1.shape[1]):
            loss+=(x_1[i,j]-x_2[i,j])**2
    loss=loss/(x_1.shape[0]*x_1.shape[0])
    return loss

# iteration EKF, which may have a worse result TAT
def EKF_iter(
    x_0:np.matrix,  # the status of last step
    P_0:np.matrix,  # the variance of last step
    y_1:np.matrix,  # the observation of this step
    u_0:np.matrix=None, # the input of system
    Phi:np.matrix=None,  # the status transform matrix 
    Psi:np.matrix=None,  # coefficient of system input
    Gamma:np.matrix=None,   # coefficient of observing noise
    R:np.matrix=None,    # the variance of observing noise
    Q:np.matrix=None,    # the variance of status noise
):
    # variable check
    if u_0 is None:
        u_0=np.matrix(0)
        print("u_0 is lacked, use default value: {}".format(u_0))
        
    if Phi is None:
        Phi=np.matrix(np.eye(x_0.shape[0]))
        print("Phi is lacked, use default value: {}".format(Phi))
    
    if Psi is None:
        Psi=np.matrix(np.zeros((x_0.shape[0],1)))
        print("Psi is lacked, use default value: {}".format(Psi))
    
    if Gamma is None:
        Gamma=np.matrix(np.zeros((x_0.shape[0],1)))
        print("Gamma is lacked, use default value: {}".format(Gamma))
        
    if R is None:
        R=np.matrix(np.zeros((y_1.shape[0],1)))
        print("R is lacked, use default value: {}".format(R))
        
    if Q is None:
        Q=np.matrix(0)
        print("Q is lacked, use default value: {}".format(Q))
        
    # one step prediction
    x_10=Phi@x_0+Psi@u_0
    P_10=Phi@P_0@Phi.T+Gamma@Q@Gamma.T
    # local linearization
    H=get_H(x_10)
    # time amend
    P_11=(P_10.I+H.T@R.I@H).I
    K_1=P_11@H.T@R.I
    x_11=x_10+K_1@(y_1-y(x_10))
    
    # iteration
    count=0
    epsilon=1e-9
    while count<500 and mse(x_11,x_10)>epsilon:
        count+=1
        x_10=x_11
        # local linearization
        H=get_H(x_10)
        # time amend
        P_11=(P_10.I+H.T@R.I@H).I
        K_1=P_11@H.T@R.I
        x_11=x_10+K_1@(y_1-y(x_10))
    return x_11,P_11,K_1


def show_result(record:dict,problem):
    pprint(record)
    for key,value in record.items():
        plt.title(key)
        plt.xlabel("step")
        if key=="h":
            plt.ylabel("高度:km")
        if key=="v":
            plt.ylabel("速度:km/s (向下为正)")
        plt.plot(value)
        plt.savefig(os.path.join(cache_dir,problem+"_"+key+".png?x-oss-process=style/webp"),dpi=600)
        plt.clf()
        plt.cla()
    

if __name__=="__main__":
    
    # solve problem 1
    x_0=np.matrix([[2],[0],[2]])
    P_0=np.matrix([[10,0,0],[0,10,0],[0,0,10]])
    x_record=[x_0]
    P_record=[P_0]
    K_record=[None]
    for obs in observation_2:
        y_1=np.matrix(obs).T
        x_1,P_1,K_1=EKF(
            x_0,
            P_0,
            y_1,
            u_0=np.matrix(9.8/1000),
            Phi=np.matrix(param_2["Phi"]),
            Psi=np.matrix(param_2["Psi"]),
            R=np.matrix(param_2["R"]),
        )
        
        x_record.append(x_1)
        P_record.append(P_1)
        K_record.append(K_1)
        
        x_0=x_1
        P_0=P_1
        
    result_record={
        "h":[x[0,0] for x in x_record],
        "v":[x[1,0] for x in x_record],
        "d_0":[x[2,0] for x in x_record],
        "P_0":[P[0,0] for P in P_record],
        "P_1":[P[1,1] for P in P_record],
        "P_2":[P[2,2] for P in P_record],
        # "K":K_record,
    }
    show_result(result_record,problem="p2")