-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtarget_decision_test.jl
135 lines (112 loc) · 4.71 KB
/
target_decision_test.jl
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
include("orbit/orbit.jl")
#include("external_model/external_model.jl")
#include("dynamic_model/dynamic_model.jl")
#include("dynamics/dynamics.jl")
#include("satellite/satellite.jl")
#include("plot/plot_plots.jl")
#include("plot/plot_makie.jl")
include("coordinates.jl")
include("satellite/target_decision.jl")
using SatelliteToolbox
using Dates
using LinearAlgebra
using Plots
gr()
using HDF5
function main()
#=
設定パラメータ
=#
DataNum =1500 #シミュレータ反復回数
dt = 30 ##シミュレータの計算間隔 [s]
start_time = DateTime(2020, 04, 09, 15, 18, 00) #シミュレート開始時刻
TLEFileName = "./orbit/ISS_TLE.txt"
# 進行方向とSCOFx軸とのずれ（内積）
dotvs = zeros(DataNum)
# 撮影地点
target_geod = [10. 10. 0.]
limit_time = 1500
# カメラパラメータ
cam_viewangle = 40
sat_axisval = 40
#=
# 軌道計算については先に行い、全 時間分を配列に保存する
JD_log, x_ecef_log, v_ecef_log, x_geod_log = orbit_cal(DataNum,dt,start_time,TLEFileName)
h5open("orbit.h5", "w") do file
write(file, "JD_log", JD_log) # alternatively, say "@write file A"
write(file, "x_ecef_log", x_ecef_log)
write(file, "v_ecef_log", v_ecef_log)
write(file, "x_geod_log", x_geod_log)
end
=#
JD_log = h5read("orbit.h5", "JD_log")
x_ecef_log = h5read("orbit.h5", "x_ecef_log")
v_ecef_log = h5read("orbit.h5", "v_ecef_log")
x_geod_log = h5read("orbit.h5", "x_geod_log")
println(size(x_ecef_log))
target_geod = [deg2rad(target_geod[1]), deg2rad(target_geod[2]), target_geod[3]]
target_ecef = GeodetictoECEF(target_geod[1], target_geod[2], target_geod[3])
print("target_geod : ")
println(target_geod)
print("target_ecef : ")
println(target_ecef)
currenttime = 1
shootingtime, shooting_vec = shootingtime_decision2(x_ecef_log[currenttime:currenttime+limit_time-1, :], target_ecef, currenttime, limit_time, cam_viewangle, sat_axisval)
println(shootingtime)
println(shooting_vec)
M = ecef_to_DCM(x_ecef_log[shootingtime,:],v_ecef_log[shootingtime,:])
shooting_vec_seof = M*shooting_vec
current_attqua = SatelliteToolbox.Quaternion(1., 0., 0., 0.)
targetqua, rotqua = targetqua_dicision(shooting_vec_seof, current_attqua, sat_axisval)
println(targetqua)
println(rotqua)
# 描画のための処理
target_geod_plot = [target_geod[1] target_geod[2]; target_geod[1] target_geod[2]]
shoot_geod_plot = [x_geod_log[shootingtime,1] x_geod_log[shootingtime,2]; x_geod_log[shootingtime,1] x_geod_log[shootingtime,2]]
shootcentor = x_ecef_log[shootingtime, :] + shooting_vec * x_geod_log[shootingtime, 3] / shooting_vec_seof[3]
shootcentor_geod = ECEFtoGeodetic(shootcentor)
cθ = dot(shooting_vec, -x_ecef_log[shootingtime, :])/(norm(shooting_vec)*norm(x_ecef_log[shootingtime, :]))
tθ = sqrt(1/(cθ^2)-1)
ta = tand(cam_viewangle/2)
tθa = (tθ+ta)/(1-tθ*ta)
shootrange = tθa * x_geod_log[shootingtime, 3] - tθ * x_geod_log[shootingtime, 3]
r = atan(shootrange / norm(shootcentor))
println(r)
shootedge = zeros(37,2)
for i = 1:37
shootedge[i, :] = [shootcentor_geod[2] + r*cosd(i*10), shootcentor_geod[1] + r*sind(i*10)]
end
#=
cθ = dot(shooting_vec, -x_ecef_log[shootingtime, :])/(norm(shooting_vec)*norm(x_ecef_log[shootingtime, :]))
tθ = sqrt(1/(cθ^2)-1)
ta = tand(cam_viewangle/2)
tθa = (tθ+ta)/(1-tθ*ta)
shootcentor_xdis = shooting_vec[1]/shooting_vec[3]*x_geod_log[shootingtime, 3]
shootcentor_ydis = shooting_vec[2]/shooting_vec[3]*x_geod_log[shootingtime, 3]
shootcentor = ECEFtoGeodetic(x_ecef_log[shootingtime, :] + [shootcentor_xdis, shootcentor_ydis, 0.])
shootrange = tθa*x_geod_log[shootingtime, 3]
shootendpoint = ECEFtoGeodetic(x_ecef_log[shootingtime, :] - [shootrange, 0., x_ecef_log[shootingtime, 3]])
Δx = shootendpoint[1] - shootcentor[1]
Δy = shootendpoint[2] - shootcentor[2]
r = sqrt(Δx^2 + Δy^2)
shootedge = zeros(37,2)
for i = 1:37
shootedge[i, :] = [shootcentor[2] + r*cosd(i*10), shootcentor[1] + r*sind(i*10)]
end
=#
# 描画
plot(x_geod_log[:,2], x_geod_log[:, 1], legend=:topleft)
plot!(target_geod_plot[:,2], target_geod_plot[:,1], seriestype=:scatter, legend=:topleft)
plot!(shoot_geod_plot[:, 2], shoot_geod_plot[:,1], seriestype=:scatter, legend=:topleft)
plot!(shootedge[:, 1], shootedge[:, 2], legend=:topleft)
xlabel!("x")
ylabel!("y")
#zlabel!("z")
savefig("./figs/TargetDecisionTest2.png")
plot(x_geod_log[shootingtime - 10:shootingtime + 10, 2],x_geod_log[shootingtime-10:shootingtime+10, 1], legend=:topleft)
plot!(target_geod_plot[:, 2], target_geod_plot[:,1], seriestype=:scatter, legend=:topleft)
plot!(shoot_geod_plot[:, 2], shoot_geod_plot[:,1], seriestype=:scatter, legend=:topleft)
plot!(shootedge[:, 1], shootedge[:, 2], legend=:topleft)
savefig("./figs/TargetDecisionTest3.png")
end
main()