-
Notifications
You must be signed in to change notification settings - Fork 1
/
example.jl
110 lines (97 loc) · 3.79 KB
/
example.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
using Plots, ControlSystemsBase, StatsBase, LaTeXStrings
using LinearAlgebra, Zygote, FileIO, JLD2, Revise
push!(LOAD_PATH, "./src")
### Loading the extended Kalman filter module ###
using eKF
### Loading information state and features maker ###
using info_feature_maker
#include("src/info_feature_state.jl")
##### Load example model ######
include("./models4example.jl")
##### Define Dynamic System Object #####
dyna = eKF.StateSpaceSys(stateDynamics, outputDynamics, Q, R)
n = dyna.n
#### Feature vector params #####
order = 1 # Highest degree of multinomial
make_feature = x -> [x; elu.(x); elu.(-x); 1.0]
make_feature = x -> [x; 1.0]
horizon = 10_000
x_true = zeros(n, horizon + 1)
x_true[:, 1] = x₀ + eKF.truncatedRandn(3, zeros(Float64, n,), dyna.Q)
#### Chose make_info_state: {1) cholesky: with_cholesky_Σ, 2) trace(QΣ): with_trace_QΣ}
infoType = "cholesky" # or trace
#### Collect features data #####
function sim2learn_eKF(x₀::Vector{Float64}, Σ₀::Matrix, U_rec::Matrix)
η₀ = make_info_state(x₀, Σ₀, dyna, infoType)
NinfoState = length(η₀)
ηₖ = zeros(NinfoState, horizon + 1)
ηₖ[:, 1] = η₀
Nfeatures = length(make_feature(η₀))
features = zeros(Nfeatures, horizon + 1)
features[:, 1] = make_feature(ηₖ[:, 1])
println("Number of features is $Nfeatures.")
println("Start collecting data ... ")
x₁, Σ₁ = x₀, Σ₀
for k = 1:horizon
u₁ = U_rec[:, k]
x_true[:, k+1] = eKF.next_state_sample(x_true[:, k], u₁, dyna)
y_true = dyna.h(x₁) # during learning, yₖ - h(xₖ) = 0 => yₖ = h(xₖ)
x₁, Σ₁ = eKF.update(x₁, Σ₁, u₁, y_true, dyna)
ηₖ[:, k+1] = make_info_state(x₁, Σ₁, dyna, infoType)
features[:, k+1] = make_feature(ηₖ[:, k+1])
end
return ηₖ, features, Nfeatures, NinfoState
end
U_rec = zeros(1, horizon)
for j=1:horizon
U_rec[1, j] = eKF.truncatedRandn(2, zeros(Float64, 1,), [0.2;;])[1]
end
#U_rec = 0.2 * randn(1, horizon)
ηₖ, features, Nfeatures, NinfoState = sim2learn_eKF(x₀, Σ₀, U_rec)
println("Features data has been collected.")
########## Learning and Control ###########
include("src/Learn_and_Sim.jl")
learnt_features, System = learn_system(features, U_rec)
simHorizon = 1000
x_DMD, U_DMD, x_lqr, U_lqr, Qₛₛ, x_true1, x_true2 =
simulate_system(System, simHorizon, x₀, Σ₀)
Qₛₛ = Qₛₛ[1:NinfoState, 1:NinfoState]
function save_to_results()
avg_cost_lqr = (LinearAlgebra.tr(Qₛₛ * x_lqr * x_lqr') + sum(U_lqr .^ 2)) / simHorizon
sse_lqr = mean((x_true2 - x_lqr[1:n, :]) .^ 2)
Result_cost1 = "Experimental cost achieved by lqr control (per k): $avg_cost_lqr"
Result_cost2 = "Experimental estimation error: $sse_lqr"
avg_cost_DMD = (LinearAlgebra.tr(Qₛₛ * x_DMD * x_DMD') + sum(U_DMD .^ 2)) / simHorizon
sse_DMD = mean((x_true1 - x_DMD[1:n, :]) .^ 2)
Result_est_err1 = "Experimental cost achieved by eDMD control (per k): $avg_cost_DMD, reduction: $((avg_cost_lqr-avg_cost_DMD)/avg_cost_lqr)"
Result_est_err2 = "Experimental estimation error: $sse_DMD, reduction: $((sse_lqr - sse_DMD)/sse_lqr)"
print(Result_cost1, "\n", Result_cost2, "\n", Result_est_err1, "\n", Result_est_err2)
open("results/results.txt", "w") do file
println(
file,
Result_cost1,
"\n",
Result_cost2,
"\n",
Result_est_err1,
"\n",
Result_est_err2,
)
end
return 1
end
save_to_results()
FileIO.save(
"results/ExampleData.jld2",
"x_lqr",
x_lqr,
"x_DMD",
x_DMD,
"n",
n,
"features",
features,
"learnt_features",
learnt_features,
)
include("plotting_paper.jl")