parabola fitting - 使用抛物线拟合离散点

Posted on Edited on In

需求

给定地对空导弹发射起始点和终止点（3维坐标），拟合导弹轨迹

思路

  • 1 首先简单化问题，假定导弹轨迹位于同一个平面内，则可以将轨迹投影到x-O-z平面，先求出x-z的关系，求一系列的散点，
    然后根据x,y的关系求出y的散点值即可
  • 2 之所以选取抛物线，因为导弹轨迹需要垂直发射的过程，存在斜率不存在的点常见的曲线为抛物线和圆，由于圆显然不合适，我们选取抛物线

实现的数学逻辑

  • 1 起始点和终止点的定义：
    1
    2
    3
    # startPos and endPos in [lon, lat, height]
    stPos = [117.9844565, 24.1658956, 1.597]
    edPos = [118.1167438, 24.5656241, 5211.73]
  • 2 数学关系
    1
    2
    3
    4
    5
    6
    7
    # 由于整个轨迹位于一个平面内，则可以考虑x，y存在线性关系
    y1 = k * x1 + b
    y2 = k * x2 + b
    则y可以由x得到

    # [x1, y1, z1]为起始点，则有如下抛物线，根据终止点[x2, y2, z2]求出p
    (z-z1)**2 = p*(x-x1)

实现代码

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


# set figure
fig = plt.figure()
ax = fig.gca(projection='3d')

################################################## input args #############################################
# startPos and endPos in [lon, lat, height]
stPos = [117.9844565, 24.1658956, 1.597]
edPos = [118.1167438, 24.5656241, 5211.73]

# # stTime, endTime, in seconds, assume it take 1 minute for missile from emit to explode
# stTime = 0
# endTime = 10

# step number
stepNumber = 30
################################################## input args #############################################

################################################## output     #############################################
# res
resX = []
resY = []
resZ = []
################################################## output     #############################################

# land defend air means, should satisfy: st.z < ed.z
# (z-z1)**2 = p*(x-x1)
def computeLnadDefendAirParabolaWithSteps(
    st,     # start point: [x, y, z]
    ed,     # end point: [x, y, z]
    resX,   # x-axis's result
    resY,   # y-axis's result
    resZ,   # z-axis's result
    stepNum # how may fitted points you want
    ):
    resX.append(st[0])
    resY.append(st[1])
    resZ.append(st[2])

    p = (ed[2] - st[2])**2 / (ed[0] - st[0])
    step = (ed[0] - st[0]) / stepNum
    for stepCnt in range(stepNum)[1:]:
        curX = st[0] + stepCnt * step
        curZ = (p * (curX - st[0])) ** 0.5 + st[2]
        resZ.append(curZ)
        # compute projection from x to y: x = k*y + b
        if st[1] != ed[1]:
            k = (st[0] - ed[0]) / (st[1] - ed[1])
            b = st[0] - k * st[1]
            curY = (curX - b) / k
            resY.append(curY)
            resX.append(curX)
        else:
            # all the y is the same
            resY.append(curY)
            resX.append(curX)
    resX.append(ed[0])
    resY.append(ed[1])
    resZ.append(ed[2])

    # print
    for idx in range(stepNum + 1):
        print("%10.6f, %10.6f, %10.3f"%(resX[idx], resY[idx], resZ[idx]))


computeLnadDefendAirParabolaWithSteps(stPos, edPos, resX, resY, resZ, stepNumber)

# 绘制图形
ax.plot(resX, resY, resZ, label='fitted curve')

# 显示图例
ax.legend()

# 显示图形
plt.show()