In [548]:
using Dierckx
using ParameterizedFunctions
using Distributions, Compat
using StatsBase
using MultivariateStats
using StaticArrays, BenchmarkTools
using DifferentialEquations
using LinearAlgebra
using Plots
using Images
using Random
using ODE
rng = MersenneTwister(1234);

Pendulum Structure Estimation

In [568]:
using DataDrivenDiffEq
using ModelingToolkit
using DiffEqSensitivity
In [569]:
function pendulum(u, p, t)
    x = u[2]
    y = -9.81sin(u[1]) - 0.1u[2]
    return [x;y]
end

u0 = [0.4π; 1.0]
tspan = (0.0, 20.0)
problem = ODEProblem(pendulum, u0, tspan)
solution = solve(problem, Tsit5(), atol = 1e-8, rtol = 1e-8, saveat = 0.001)

X = Array(solution)
DX = solution(solution.t, Val{1});
In [570]:
plot(X[1,:])
plot!(X[2,:])
Out[570]:
0 5.0×10 3 1.0×10 4 1.5×10 4 2.0×10 4 -3 -2 -1 0 1 2 3 y1 y2
In [571]:
@variables u[1:2]

h = Operation[u; u.^2; u.^3; sin.(u); cos.(u); 1]

basis = Basis(h, u)
Out[571]:
11 dimensional basis in ["u₁", "u₂"]
In [572]:
opt = SR3(3e-1, 1.0)
Ψ = SInDy(X[:, 1:1000], DX[:, 1:1000], basis, maxiter = 10000, opt = opt, normalize = true)
Out[572]:
2 dimensional basis in ["u₁", "u₂"]
In [573]:
sys = ODESystem(Ψ)
p = parameters(Ψ)

dudt = ODEFunction(sys)

estimator = ODEProblem(dudt, u0, tspan, p)
estimation = solve(estimator, Tsit5(), saveat = solution.t);
Xest=Array(estimation)
Out[573]:
2×20001 Array{Float64,2}:
 1.25664  1.25763  1.25861   1.25958   …  -0.43326   -0.434091  -0.434917
 1.0      0.99059  0.981178  0.971764     -0.837031  -0.832838  -0.828639
In [576]:
println(Ψ)

2 dimensional basis in ["u₁", "u₂"]
du₁ = u₂ * 0.9944866064389278
du₂ = u₂ * -0.08978215064914737 + sin(u₁) * -9.798766377092148
In [575]:
plot(X[1,:])
plot!(X[2,:])
plot!(Xest[1,:],linestyle = :dash)
plot!(Xest[2,:],linestyle = :dash)
Out[575]:
0 5.0×10 3 1.0×10 4 1.5×10 4 2.0×10 4 -3 -2 -1 0 1 2 3 y1 y2 y3 y4 
2 dimensional basis in ["u₁", "u₂"]
du₁ = u₂ * 0.9944866064389278
du₂ = u₂ * -0.08978215064914737 + sin(u₁) * -9.798766377092148
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [496]:
# Differential equations describing the system
function double_pendulum(du,u,p,t)
    # du = derivatives
    # u = variables
    # p = parameters
    # t = time variable
    
    m1 = p[1];
    m2 = p[2];
    L1 = p[3];
    L2 = p[4];
    g = p[5];
  
    
    c = cos(u[1]-u[3]);  # intermediate variables
    s = sin(u[1]-u[3]);  # intermediate variables

    
    du[1] = u[2];   # d(theta 1)
    du[2] = ( m2*g*sin(u[3])*c - m2*s*(L1*c*u[2]^2 + L2*u[4]^2) - (m1+m2)*g*sin(u[1]) ) /( L1 *(m1+m2*s^2) );
    du[3] = u[4];   # d(theta 2)   
    du[4] = ((m1+m2)*(L1*u[2]^2*s - g*sin(u[3]) + g*sin(u[1])*c) + m2*L2*u[4]^2*s*c) / (L2 * (m1 + m2*s^2));
    
end
Out[496]:
double_pendulum (generic function with 1 method)
In [535]:
m1 = 1;                 # mass of pendulum 1 (in kg)
m2 = 1;                 # mass of pendulum 2 (in kg)
l1 = 1;                 # length of pendulum 1 (in meter)
l2 = 1;                 # length of pendulum 2 (in meter)
g = 9.8;                # gravitatioanl acceleration constant (m/s^2)
duration = 20.0;
dt = 0.001;



u0 = [pi/2; 0;pi/2; 0];    # initial conditions. 
# u[1] = angle of the first pendulum
# u[2] = angular velocity of the first pendulum
# u[3] = angle of the second pendulum
# u[4] = angular velocity of the second pendulum

tfinal = duration;       # Final time. Simulation time = 0 to tfinal.

# Solving the system

p = [m1;  m2;  l1;  l2;  g];


tspan = (0.0,tfinal);  # Time span (limits). The actual time variable is automatically set by solve().
problem = ODEProblem(double_pendulum,u0,tspan,p);
solution = solve(problem,Vern7(),saveat=dt);

# sol[1,:] = u1 = Θ_1
# sol[2,:] = u2 = ω_1
# sol[3,:] = u3 = Θ_2
# sol[4,:] = u4 = ω_2
#sol.t = t = time variable. The ODE solver automatically chooses the time steps.

ph1 = solution[1,:]
ph1dot = solution[2,:]
ph2 = solution[3,:]
ph2dot = solution[4,:]

# anim=plotSolution(solution,p)
# gif(anim,fps=10)
Out[535]:
20001-element Array{Float64,1}:
  0.0                   
 -2.8235669336227366e-13
 -9.03544952672869e-12  
 -6.861291600120947e-11 
 -2.891341897656128e-10 
 -8.823674853841984e-10 
 -2.195612607491098e-9  
 -4.745583780355568e-9  
 -9.252292424507022e-9  
 -1.6672929777018045e-8 
 -2.823574925209158e-8  
 -4.5473948438503673e-8 
 -7.025955063051468e-8  
  ⋮                     
  1.9579201203519148    
  1.9634969102725919    
  1.9691526769469827    
  1.9748868374892472    
  1.9806987985169695    
  1.9865879565922178    
  1.9925536986765469    
  1.9985954026005868    
  2.0047124375478176    
  2.0109041645537404    
  2.0171699370186977    
  2.023509101236595
In [536]:
solution = solve(problem, Tsit5(), saveat = dt)
X = Array(solution)
DX = solution(solution.t, Val{1});
In [537]:
plot(X[1,:])
plot!(X[2,:])
plot!(X[3,:])
plot!(X[4,:])
Out[537]:
0 5.0×10 3 1.0×10 4 1.5×10 4 2.0×10 4 -15 -10 -5 0 5 y1 y2 y3 y4
In [538]:
X[:, 1:length(solution.t)]
u0
Out[538]:
4-element Array{Float64,1}:
 1.5707963267948966
 0.0               
 1.5707963267948966
 0.0
In [539]:
DX[:, 1:length(solution.t)]
Out[539]:
4×20001 Array{Float64,2}:
 -0.0049       -0.0049       -0.0147       …    2.46451    2.50186    2.53943
 -9.8          -9.8          -9.8              37.2252    37.418     37.6049 
  0.0           0.0          -1.44551e-10      -8.51789   -8.55774   -8.59804
 -2.76195e-10  -2.76195e-10  -3.04886e-8      -39.6062   -40.0318   -40.4533
In [540]:
@variables u[1:4]

h = Operation[u; sin.(u); cos.(u); sin.(u.^2); cos.(u.^2); 1]

basis = Basis(h, u)
Out[540]:
21 dimensional basis in ["u₁", "u₂", "u₃", "u₄"]
In [541]:
opt = SR3(3e-1, 1.0)
Ψ = SInDy(X[:, :], DX[:, :], basis, maxiter = 1000000, opt = opt, normalize = true)
Out[541]:
4 dimensional basis in ["u₁", "u₂", "u₃", "u₄"]
In [542]:
println(Ψ)

4 dimensional basis in ["u₁", "u₂", "u₃", "u₄"]
du₁ = u₂ * 0.9996802103589715 + sin(u₁) * 0.003584380125796723 + sin(u₃) * -0.0037471066473492524
du₂ = u₁ * -8.011110961840414 + u₂ * 0.25119559225240556 + u₃ * -0.07204938073298425 + u₄ * -0.06179744701384284 + sin(u₁) * -6.985888061734992 + sin(u₂) * 0.05818449583369576 + sin(u₃) * 11.773693317404312 + sin(u₄) * -0.11416963695872571 + cos(u₁) * 7.219847080777781 + cos(u₂) * 0.8750390072744998 + cos(u₃) * -0.7632767887325713 + cos(u₄) * -0.36227382974591543 + sin(u₁ ^ 2) * 1.6636128641898378 + sin(u₂ ^ 2) * -0.2074123979198855 + sin(u₃ ^ 2) * 0.22988182315574912 + sin(u₄ ^ 2) * -0.8165687268228538 + cos(u₁ ^ 2) * -3.183388106803286 + cos(u₂ ^ 2) * -0.21263477222060495 + cos(u₃ ^ 2) * 0.35975160246325233 + cos(u₄ ^ 2) * 0.870518177796281 + -3.662669314113484
du₃ = u₄ * 0.9997804635917078 + sin(u₃) * 0.005787194137009694
du₄ = u₁ * 21.890604797144036 + u₂ * -0.2266659790388553 + u₃ * -0.16491462747110272 + u₄ * 0.07500819530618195 + sin(u₁) * -20.553655620460354 + sin(u₂) * -0.20361672892728128 + sin(u₃) * -15.880138152132773 + sin(u₄) * 1.2154937688539635 + cos(u₁) * -20.076414414261215 + cos(u₂) * 0.35367180705833656 + cos(u₃) * 3.322528104066608 + cos(u₄) * 0.9233342592322777 + sin(u₁ ^ 2) * -3.819196883769029 + sin(u₂ ^ 2) * -0.2706311183319416 + sin(u₃ ^ 2) * -0.5135749942772934 + sin(u₄ ^ 2) * 1.3812379491037985 + cos(u₁ ^ 2) * 10.114104257338271 + cos(u₂ ^ 2) * 0.11211972256810503 + cos(u₃ ^ 2) * -0.941811643061641 + cos(u₄ ^ 2) * -0.9431719262851088 + 8.266740251635646
In [543]:
sys = ODESystem(Ψ)
p = parameters(Ψ)

dudt = ODEFunction(sys)

estimator = ODEProblem(dudt, u0, tspan, p)
estimation = solve(estimator, Tsit5(), saveat = solution.t);
Xest=Array(estimation)
Out[543]:
4×20001 Array{Float64,2}:
 1.5708   1.57079      1.57078     …  -146.444  -146.273  -146.102
 0.0     -0.00701409  -0.0140297       169.789   171.007   172.234
 1.5708   1.5708       1.5708          412.302   411.693   411.081
 0.0     -0.00346554  -0.00693279     -607.758  -611.074  -614.402
In [544]:
println(Ψ)

4 dimensional basis in ["u₁", "u₂", "u₃", "u₄"]
du₁ = u₂ * 0.9996802103589715 + sin(u₁) * 0.003584380125796723 + sin(u₃) * -0.0037471066473492524
du₂ = u₁ * -8.011110961840414 + u₂ * 0.25119559225240556 + u₃ * -0.07204938073298425 + u₄ * -0.06179744701384284 + sin(u₁) * -6.985888061734992 + sin(u₂) * 0.05818449583369576 + sin(u₃) * 11.773693317404312 + sin(u₄) * -0.11416963695872571 + cos(u₁) * 7.219847080777781 + cos(u₂) * 0.8750390072744998 + cos(u₃) * -0.7632767887325713 + cos(u₄) * -0.36227382974591543 + sin(u₁ ^ 2) * 1.6636128641898378 + sin(u₂ ^ 2) * -0.2074123979198855 + sin(u₃ ^ 2) * 0.22988182315574912 + sin(u₄ ^ 2) * -0.8165687268228538 + cos(u₁ ^ 2) * -3.183388106803286 + cos(u₂ ^ 2) * -0.21263477222060495 + cos(u₃ ^ 2) * 0.35975160246325233 + cos(u₄ ^ 2) * 0.870518177796281 + -3.662669314113484
du₃ = u₄ * 0.9997804635917078 + sin(u₃) * 0.005787194137009694
du₄ = u₁ * 21.890604797144036 + u₂ * -0.2266659790388553 + u₃ * -0.16491462747110272 + u₄ * 0.07500819530618195 + sin(u₁) * -20.553655620460354 + sin(u₂) * -0.20361672892728128 + sin(u₃) * -15.880138152132773 + sin(u₄) * 1.2154937688539635 + cos(u₁) * -20.076414414261215 + cos(u₂) * 0.35367180705833656 + cos(u₃) * 3.322528104066608 + cos(u₄) * 0.9233342592322777 + sin(u₁ ^ 2) * -3.819196883769029 + sin(u₂ ^ 2) * -0.2706311183319416 + sin(u₃ ^ 2) * -0.5135749942772934 + sin(u₄ ^ 2) * 1.3812379491037985 + cos(u₁ ^ 2) * 10.114104257338271 + cos(u₂ ^ 2) * 0.11211972256810503 + cos(u₃ ^ 2) * -0.941811643061641 + cos(u₄ ^ 2) * -0.9431719262851088 + 8.266740251635646
In [547]:
plot(X[1,1:10000])
plot!(X[2,1:10000])
# plot!(X[3,:])
# plot!(X[4,:])
plot!(Xest[1,1:10000],linestyle = :dash)
plot!(Xest[2,1:10000],linestyle = :dash)
# plot!(Xest[3,:],linestyle = :dash)
# plot!(Xest[4,:],linestyle = :dash)
Out[547]:
0 2500 5000 7500 10000 -20 0 20 40 y1 y2 y3 y4
In [ ]: