【线】轨迹平滑

滑动平均平滑

邻域内的数据点做平均代替邻域的中心点值,除了一般滑动平均,还有加权滑动平均和指数滑动平均。

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
import numpy as np
from matplotlib import pyplot as plt

def MovingAverage(input):
output = {}
size = len(input)
i = 0
output[0] = (32.0 * input[0] + 15.0 * input[1] + 3.0 * input[2] - 4.0 * input[3] - 6.0 * input[4] - 3.0 * input[5] + 5.0 * input[6]) / 42.0
output[1] = (5.0 * input[0] + 4.0 * input[1] + 3.0 * input[2] + 2.0 * input[3] + input[4] - input[6]) / 14.0
output[2] = (1.0 * input[0] + 3.0 * input[1] + 4.0 * input[2] + 4.0 * input[3] + 3.0 * input[4] + 1.0 * input[5] - 2.0 * input[6]) / 14.0

for i in range(3, size - 3):
output[i] = (-2.0 * (input[i - 3] + input[i + 3]) + 3.0 * (input[i - 2] + input[i + 2]) + 6.0 * (input[i - 1] + input[i + 1]) + 7.0 * input[i]) / 21.0

output[size - 3] = (1.0 * input[size - 1] + 3.0 * input[size - 2] + 4.0 * input[size - 3] + 4.0 * input[size - 4] + 3.0 * input[size - 5] + 1.0 * input[size - 6] - 2.0 * input[size - 7]) / 14.0
output[size - 2] = (5.0 * input[size - 1] + 4.0 * input[size - 2] + 3.0 * input[size - 3] + 2.0 * input[size - 4] + input[size - 5] - input[size - 7]) / 14.0
output[size - 1] = (32.0 * input[size - 1] + 15.0 * input[size - 2] + 3.0 * input[size - 3] - 4.0 * input[size - 4] - 6.0 * input[size - 5] - 3.0 * input[size - 6] + 5.0 * input[size - 7]) / 42.0
return output

if __name__ == '__main__':
x = np.linspace(1, 30, 30)

# 原始数据
y0 = [1, 2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 12, 13, 11, 12, 14, 18, 19, 21, 22, 22, 23, 24, 25, 26, 27, 28, 29]
# 平滑后数据
y1 = list(MovingAverage(y0).values())

plt.scatter(x, y0, s=75, color="red", alpha=1)
plt.scatter(x, y1, s=75, color="blue", alpha=1)
plt.show()

卡尔曼滤波

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
import numpy as np
import matplotlib.pyplot as plt

def kalman_filter(position_noise):
# 初始的估计位置就直接用测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = 120**2 # 这是我们自己设定的位置测量仪器的方差,越大则测量值占比越低
v_std = 50 # 测量仪器的方差

for i in range(1, t.shape[0]):
dv = (position[i] - position[i-1]) + np.random.normal(0, 50) # 模拟仪器读取出的速度
position_predict = position_predict + dv # 利用上个时刻的位置和速度预测当前位置
predict_var += v_std**2 # 更新预测数据的方差
# 下面是Kalman滤波
position_predict = position_predict*odo_var / (predict_var + odo_var) + position_noise[i]*predict_var/(predict_var + odo_var)
predict_var = (predict_var * odo_var) / (predict_var + odo_var)**2
predicts.append(position_predict)

return predicts

if __name__ == '__main__':
# 模拟数据
t = np.linspace(1, 100, 100)
position = (0.5 * t**2)/2
position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))

# 卡尔曼滤波
predicts = kalman_filter(position_noise)

plt.plot(t, position, label='truth position')
plt.plot(t, position_noise, label='only use measured position')
plt.plot(t, predicts, label='kalman filtered position')
plt.legend()
plt.show()
0%