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);

http://essg.mit.edu/ml2020

In [549]:
# 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[549]:
double_pendulum (generic function with 1 method)
In [550]:
function plotSolution(sol,par)
    # Extract the variables from the solution 
    
    L1 = par[2]  #First length
    L2 = par[4]  #Second length

    tm = sol.t;

    # Mapping from polar to Cartesian
    x1_u = L1 *sin.(sol[1,:]);          # First Pendulum
    y1_u = -L1 *cos.(sol[1,:]);

    x2_u = x1_u + L2*sin.(sol[3,:]);     # Second Pendulum
    y2_u = y1_u - L2*cos.(sol[3,:]);
    


    
    L = L1 + L2;
    axis_lim = L*1.2;   # defining the limits of the axes

    anim = Animation()
    
    i=1
    extrasteps=10
    for j in 1:extrasteps
        str = string("Time = ", round(tm[i],digits=1), " sec");
        


        plot([0,x1_u[i]], [0,y1_u[i]],size=(400,300),xlim=(-axis_lim,axis_lim),ylim=(-axis_lim,1),markersize = 10, markershape = :circle,label ="Real",axis = [],color = :blue);
        plot!([x1_u[i],x2_u[i]], [y1_u[i],y2_u[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :blue);

        frame(anim)
    end
    
    ev=10
    laststep=0
    for i =1:ev:length(sol.t)
        str = string("Time = ", round(tm[i],digits=1), " sec");
        


        plot([0,x1_u[i]], [0,y1_u[i]],size=(400,300),xlim=(-axis_lim,axis_lim),ylim=(-axis_lim,1),markersize = 10, markershape = :circle,label ="Real",axis = [],color = :blue);
        plot!([x1_u[i],x2_u[i]], [y1_u[i],y2_u[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :blue);
        

        if i > 9*ev
            plot!([x2_u[i-3*ev:i]],      [y2_u[i-3*ev:i]]     ,alpha = 0.15,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-5*ev:i-3*ev]], [y2_u[i-5*ev:i-3*ev]],alpha = 0.08,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-7*ev:i-5*ev]], [y2_u[i-7*ev:i-5*ev]],alpha = 0.04,linewidth = 2, color = :blue, label ="");
            plot!([x2_u[i-9*ev:i-7*ev]], [y2_u[i-9*ev:i-7*ev]],alpha = 0.01,linewidth = 2, color = :blue, label="");
            

        end
        laststep=i
        frame(anim)
    end 
    
    i=laststep
    extrasteps=20
    for j in 1:extrasteps
        str = string("Time = ", round(tm[i],digits=1), " sec");
        


        plot([0,x1_u[i]], [0,y1_u[i]],size=(400,300),xlim=(-axis_lim,axis_lim),ylim=(-axis_lim,1),markersize = 10, markershape = :circle,label ="Real",axis = [],color = :blue);
        plot!([x1_u[i],x2_u[i]], [y1_u[i],y2_u[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :blue);
        

        if i > 9*ev
            plot!([x2_u[i-3*ev:i]],      [y2_u[i-3*ev:i]]     ,alpha = 0.15,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-5*ev:i-3*ev]], [y2_u[i-5*ev:i-3*ev]],alpha = 0.08,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-7*ev:i-5*ev]], [y2_u[i-7*ev:i-5*ev]],alpha = 0.04,linewidth = 2, color = :blue, label ="");
            plot!([x2_u[i-9*ev:i-7*ev]], [y2_u[i-9*ev:i-7*ev]],alpha = 0.01,linewidth = 2, color = :blue, label="");
            

        end
        frame(anim)
    end
    
    return anim
end
Out[550]:
plotSolution (generic function with 3 methods)
In [551]:
function plotSolution(sol,linSol,par)
    # Extract the variables from the solution 
    
    L1 = par[2]  #First length
    L2 = par[4]  #Second length

    tm = sol.t;

    # Mapping from polar to Cartesian
    x1_u = L1 *sin.(sol[1,:]);          # First Pendulum
    y1_u = -L1 *cos.(sol[1,:]);

    x2_u = x1_u + L2*sin.(sol[3,:]);     # Second Pendulum
    y2_u = y1_u - L2*cos.(sol[3,:]);
    
    x1_u1 = L1 *sin.(linSol[1,:]);          # First Pendulum
    y1_u1 = -L1 *cos.(linSol[1,:]);

    x2_u1 = x1_u1 + L2*sin.(linSol[3,:]);     # Second Pendulum
    y2_u1 = y1_u1 - L2*cos.(linSol[3,:]);

    
    L = L1 + L2;
    axis_lim = L*1.2;   # defining the limits of the axes

    anim = Animation()
    
    i=1
    extrasteps=10
    for j in 1:extrasteps
        str = string("Time = ", round(tm[i],digits=1), " sec");
        
        plot([0,x1_u1[i]], [0,y1_u1[i]],size=(400,300),markersize = 10, markershape = :circle,label ="Fitted",axis = [],color = :orange);
        plot!([x1_u1[i],x2_u1[i]], [y1_u1[i],y2_u1[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :orange);


        plot!([0,x1_u[i]], [0,y1_u[i]],size=(400,300),xlim=(-axis_lim,axis_lim),ylim=(-axis_lim,1),markersize = 10, markershape = :circle,label ="Real",axis = [],color = :blue);
        plot!([x1_u[i],x2_u[i]], [y1_u[i],y2_u[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :blue);

        frame(anim)
    end
    
    ev=10
    laststep=0
    for i =1:ev:length(sol.t)
        str = string("Time = ", round(tm[i],digits=1), " sec");
        
        plot([0,x1_u1[i]], [0,y1_u1[i]],size=(400,300),markersize = 10, markershape = :circle,label ="Fitted",axis = [],color = :orange);
        plot!([x1_u1[i],x2_u1[i]], [y1_u1[i],y2_u1[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :orange);


        plot!([0,x1_u[i]], [0,y1_u[i]],size=(400,300),xlim=(-axis_lim,axis_lim),ylim=(-axis_lim,1),markersize = 10, markershape = :circle,label ="Real",axis = [],color = :blue);
        plot!([x1_u[i],x2_u[i]], [y1_u[i],y2_u[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :blue);
        

        if i > 9*ev
            plot!([x2_u[i-3*ev:i]],      [y2_u[i-3*ev:i]]     ,alpha = 0.15,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-5*ev:i-3*ev]], [y2_u[i-5*ev:i-3*ev]],alpha = 0.08,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-7*ev:i-5*ev]], [y2_u[i-7*ev:i-5*ev]],alpha = 0.04,linewidth = 2, color = :blue, label ="");
            plot!([x2_u[i-9*ev:i-7*ev]], [y2_u[i-9*ev:i-7*ev]],alpha = 0.01,linewidth = 2, color = :blue, label="");
            
            plot!([x2_u1[i-3*ev:i]],   [y2_u1[i-3*ev:i]],alpha = 0.15,linewidth = 2, color = :orange,label ="");
            plot!([x2_u1[i-5*ev:i-3*ev]], [y2_u1[i-5*ev:i-3*ev]],alpha = 0.08,linewidth = 2, color = :orange,label ="");
            plot!([x2_u1[i-7*ev:i-5*ev]], [y2_u1[i-7*ev:i-5*ev]],alpha = 0.04,linewidth = 2, color = :orange, label ="");
            plot!([x2_u1[i-9*ev:i-7*ev]], [y2_u1[i-9*ev:i-7*ev]],alpha = 0.01,linewidth = 2, color = :orange, label="");
        end
        laststep=i
        frame(anim)
    end 
    
    i=laststep
    extrasteps=20
    for j in 1:extrasteps
        str = string("Time = ", round(tm[i],digits=1), " sec");
        
        plot([0,x1_u1[i]], [0,y1_u1[i]],size=(400,300),markersize = 10, markershape = :circle,label ="Fitted",axis = [],color = :orange);
        plot!([x1_u1[i],x2_u1[i]], [y1_u1[i],y2_u1[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :orange);


        plot!([0,x1_u[i]], [0,y1_u[i]],size=(400,300),xlim=(-axis_lim,axis_lim),ylim=(-axis_lim,1),markersize = 10, markershape = :circle,label ="Real",axis = [],color = :blue);
        plot!([x1_u[i],x2_u[i]], [y1_u[i],y2_u[i]],markersize = 10, markershape = :circle,label ="",title = str, title_location = :left, aspect_ratio = :equal,color = :blue);
        

        if i > 9*ev
            plot!([x2_u[i-3*ev:i]],      [y2_u[i-3*ev:i]]     ,alpha = 0.15,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-5*ev:i-3*ev]], [y2_u[i-5*ev:i-3*ev]],alpha = 0.08,linewidth = 2, color = :blue,label ="");
            plot!([x2_u[i-7*ev:i-5*ev]], [y2_u[i-7*ev:i-5*ev]],alpha = 0.04,linewidth = 2, color = :blue, label ="");
            plot!([x2_u[i-9*ev:i-7*ev]], [y2_u[i-9*ev:i-7*ev]],alpha = 0.01,linewidth = 2, color = :blue, label="");
            
            plot!([x2_u1[i-3*ev:i]],   [y2_u1[i-3*ev:i]],alpha = 0.15,linewidth = 2, color = :orange,label ="");
            plot!([x2_u1[i-5*ev:i-3*ev]], [y2_u1[i-5*ev:i-3*ev]],alpha = 0.08,linewidth = 2, color = :orange,label ="");
            plot!([x2_u1[i-7*ev:i-5*ev]], [y2_u1[i-7*ev:i-5*ev]],alpha = 0.04,linewidth = 2, color = :orange, label ="");
            plot!([x2_u1[i-9*ev:i-7*ev]], [y2_u1[i-9*ev:i-7*ev]],alpha = 0.01,linewidth = 2, color = :orange, label="");
        end
        frame(anim)
    end
    
    return anim
end
Out[551]:
plotSolution (generic function with 3 methods)
In [552]:
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 = 10.0;
dt = 0.01;



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().
prob = ODEProblem(double_pendulum,u0,tspan,p);
sol0 = solve(prob,Vern7(),dt=dt,minstep=dt,dtmax=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 = sol0[1,:]
ph1dot = sol0[2,:]
ph2 = sol0[3,:]
ph2dot = sol0[4,:]

x = Array(sol0)'
size(x)[1]
anim=plotSolution(sol0,p)
gif(anim,fps=10)

┌ Info: Saved animation to 
│   fn = C:\Users\amira\Dropbox (MIT)\CBA\_Classes\4_Spring 2020\Machine Learning System Optimization\Project\tmp.gif
└ @ Plots C:\Users\amira\.julia\packages\Plots\cc8wh\src\animation.jl:98
Out[552]:
In [553]:
# Measurements
H = 5:5:(size(x)[1]);
lH = length(H);
ph1_obs = ph1[H]+randn(lH,1)*5/180*pi;
ph2_obs = ph2[H]+randn(lH,1)*5/180*pi;
ph_obs = [ph1_obs ph2_obs];
In [554]:
# Random Ensemble Simulation
xinit_ens = u0[1:4] .+ randn(4,100) .* 5.0 ./ 180.0 .*pi
# xinit_ens = [xinit_ens; repeat(p[1:4],1,100)]
Out[554]:
4×100 Array{Float64,2}:
  1.60397    1.5019      1.53288   …   1.56294    1.44856      1.5965   
 -0.0135407  0.0515468  -0.121466      0.154986  -0.00655098  -0.0454943
  1.50117    1.49553     1.50942       1.44973    1.61348      1.65341  
  0.115971   0.147461   -0.151296     -0.100532   0.1189      -0.146253
In [555]:
plot(ph1,ph2,label="True")
scatter!(ph1_obs,ph2_obs,label="Obs")
scatter!(xinit_ens[1,:], xinit_ens[3,:],label="Ens")
Out[555]:
-1.5 -1.0 -0.5 0.0 0.5 1.0 1.5 -2 -1 0 1 2 3 True Obs Ens
In [556]:
x_ens=zeros(size(x)[1],size(xinit_ens)[1],size(xinit_ens)[2])
for ens in 1:size(xinit_ens)[2]
    prob = ODEProblem(double_pendulum,xinit_ens[:,ens],tspan,p);
    
    sol=solve(prob,Vern7(),dt=dt,minstep=dt,dtmax=dt);
    
    x = Array(sol)'
    x_ens[:,:,ens] .= x;
end
In [557]:
size(x_ens)
Out[557]:
(1002, 4, 100)
In [558]:
H[1]
Out[558]:
5
In [559]:
anim=Animation()
for tm in 1:size(x_ens)[1]
    x_this =x_ens[tm,:,:]
    plot(ph1,ph2,label="",xlims = (-3,3),ylims = (-4,6))
    scatter!(x_this[1,:],x_this[3,:],label="")
    frame(anim)
end
gif(anim,fps=30)

┌ Info: Saved animation to 
│   fn = C:\Users\amira\Dropbox (MIT)\CBA\_Classes\4_Spring 2020\Machine Learning System Optimization\Project\tmp.gif
└ @ Plots C:\Users\amira\.julia\packages\Plots\cc8wh\src\animation.jl:98
Out[559]:
In [ ]:
$$ E_2^+=E_2 +E_2 h(E_2)^T[h(E_2)h(E_2)^T+R]^{-1}(Y-h(E_2)) $$
$$ E_2^+=E_2 +E_z E_z^T[E_z E_z^T+R]^{-1}(Y-E_z) $$
$$ E_z=h(E_2)$$$$ E_2=F(E_1)$$$$E_2^+=E_1+\delta E_z$$

Prediction Error: $$ \delta Y= (Y-E_z) $$ Correction: $$ \delta E_z=E_z E_z^T[E_z E_z^T+R]^{-1}\delta Y $$

Empirical Adjoint: $$ \delta E_1=E_1 E_2^T[E_2 E_2^T+R]^{-1}\delta E_2 $$

$E_1 \leftarrow E_2$ using $F()$ the go back with the adjoint $$E_{\alpha}M_2$$

In [560]:
x_ens[H[1],:,:]
Out[560]:
4×100 Array{Float64,2}:
  1.59561    1.49614    1.52018   …   1.56129     1.44068    1.58683 
 -0.404476  -0.339528  -0.51357      -0.237441   -0.387166  -0.437678
  1.50577    1.50143    1.50339       1.44572     1.61792    1.64757 
  0.114143   0.147555  -0.150378     -0.0996956   0.102081  -0.14564
In [561]:
H_obs = [1 0 0 0; 0 0 1 0];
x_ens_meas = x_ens[H[1],:,:]
x = u0;
anim=Animation()
sta_upd=[]
for tm in 1:2:length(H)
    
    plot(ph1,ph2,label="",xlims = (-2,2),ylims = (-2,4))
    scatter!(x_ens_meas[1,:], x_ens_meas[3,:],label="")
    frame(anim)
    
    sta_var = x_ens_meas[1:4,:];
    
    covstat = cov(sta_var');
    
    sta_upd = sta_var + (covstat *H_obs') * (pinv(H_obs*covstat*H_obs' +(zeros(2,2) + I)*(2/180*pi)^2) * (ph_obs[tm,:] .- (H_obs*sta_var)))
  
#     sta_upd = sta_var;
    for ens in 1:size(sta_upd)[2]
#         println(sta_upd[:,ens])
        
        prob = ODEProblem(double_pendulum,sta_upd[:,ens],(0.0,0.1),p);
        sol=solve(prob,Vern7(),saveat=0.1);
    
        x = Array(sol)
    
        
        x_ens_meas[:,ens] .= x[:,end]

    end
#     print(x_ens_meas[1,:])
    

end
gif(anim,fps=5)

┌ Info: Saved animation to 
│   fn = C:\Users\amira\Dropbox (MIT)\CBA\_Classes\4_Spring 2020\Machine Learning System Optimization\Project\tmp.gif
└ @ Plots C:\Users\amira\.julia\packages\Plots\cc8wh\src\animation.jl:98
Out[561]:

In [562]:
x_ens
Out[562]:
1002×4×100 Array{Float64,3}:
[:, :, 1] =
  1.60397     -0.0135407   1.50117    0.115971
  1.60334     -0.111182    1.50233    0.115343
  1.60174     -0.208866    1.50348    0.114793
  1.59916     -0.306621    1.50463    0.114377
  1.59561     -0.404476    1.50577    0.114143
  1.59107     -0.502448    1.50691    0.114127
  1.58556     -0.600549    1.50805    0.114348
  1.57906     -0.698776    1.5092     0.114809
  1.57158     -0.797118    1.51035    0.115487
  1.56312     -0.895544    1.51151    0.116335
  1.55367     -0.994009    1.51268    0.117275
  1.54324     -1.09245     1.51385    0.118195
  1.53182     -1.19077     1.51504    0.118945
  ⋮                                           
  0.185465    -2.58417    -0.855126  -4.99291 
  0.158733    -2.76016    -0.903933  -4.76804 
  0.130303    -2.92388    -0.950477  -4.5403  
  0.100296    -3.07553    -0.994728  -4.30915 
  0.0688314   -3.21539    -1.03665   -4.07421 
  0.0360262   -3.34378    -1.0762    -3.83533 
  0.00199265  -3.46113    -1.11334   -3.5925  
 -0.0331611   -3.56792    -1.14804   -3.34587 
 -0.0693325   -3.66473    -1.18025   -3.09571 
 -0.106425    -3.75221    -1.20994   -2.84236 
 -0.144348    -3.83106    -1.23708   -2.58627 
 -0.144348    -3.83106    -1.23708   -2.58627 

[:, :, 2] =
  1.5019     0.0515468   1.49553   0.147461
  1.50193   -0.0462613   1.497     0.147539
  1.50097   -0.144053    1.49848   0.147592
  1.49905   -0.241815    1.49995   0.147605
  1.49614   -0.339528    1.50143   0.147555
  1.49225   -0.437167    1.50291   0.14741 
  1.4874    -0.534702    1.50438   0.147123
  1.48156   -0.632089    1.50585   0.14663 
  1.47475   -0.729274    1.50731   0.145845
  1.46698   -0.82619     1.50876   0.144662
  1.45823   -0.922756    1.5102    0.142946
  1.44852   -1.01887     1.51162   0.140536
  1.43786   -1.11443     1.51301   0.137242
  ⋮                                        
 -0.338641  -4.21815    -1.10038  -0.879383
 -0.38113   -4.27926    -1.1079   -0.624599
 -0.424218  -4.33777    -1.11287  -0.37165 
 -0.467878  -4.39382    -1.11534  -0.121598
 -0.512086  -4.44734    -1.11532   0.124215
 -0.556815  -4.49791    -1.11287   0.364073
 -0.602031  -4.54472    -1.10807   0.59578 
 -0.647692  -4.58645    -1.10099   0.816561
 -0.693737  -4.62127    -1.09178   1.023   
 -0.740087  -4.6468     -1.0806    1.21106 
 -0.786633  -4.66021    -1.06764   1.37623 
 -0.786633  -4.66021    -1.06764   1.37623 

[:, :, 3] =
 1.53288    -0.121466    1.50942  -0.151296
 1.53117    -0.219494    1.50791  -0.151099
 1.52849    -0.317524    1.5064   -0.150879
 1.52482    -0.415553    1.50489  -0.150636
 1.52018    -0.51357     1.50339  -0.150378
 1.51455    -0.611558    1.50189  -0.150127
 1.50795    -0.70949     1.50038  -0.149919
 1.50036    -0.807329    1.49889  -0.149807
 1.4918     -0.905025    1.49739  -0.149868
 1.48226    -1.00251     1.49589  -0.150204
 1.47175    -1.09971     1.49438  -0.150942
 1.46027    -1.19652     1.49287  -0.152243
 1.44782    -1.29282     1.49134  -0.154299
 ⋮                                         
 0.181801   -0.0932072  -1.48812  -6.13754 
 0.179875   -0.29069    -1.54912  -6.06463 
 0.176012   -0.480851   -1.60944  -6.0004  
 0.170282   -0.663999   -1.66915  -5.94326 
 0.162755   -0.840298   -1.72832  -5.8918  
 0.153498   -1.00979    -1.787    -5.84476 
 0.142582   -1.17243    -1.84523  -5.80102 
 0.130073   -1.32809    -1.90303  -5.75958 
 0.116044   -1.47659    -1.96042  -5.71958 
 0.100566   -1.61774    -2.01742  -5.68023 
 0.0837141  -1.75131    -2.07403  -5.64087 
 0.0837141  -1.75131    -2.07403  -5.64087 

...

[:, :, 98] =
  1.56294    0.154986    1.44973   -0.100532 
  1.564      0.0568849   1.44872   -0.100334 
  1.56408   -0.0411995   1.44772   -0.100171 
  1.56318   -0.139298    1.44672   -0.0999797
  1.56129   -0.237441    1.44572   -0.0996956
  1.55843   -0.335659    1.44473   -0.099257 
  1.55458   -0.433977    1.44374   -0.0986086
  1.54975   -0.532414    1.44276   -0.0977046
  1.54393   -0.630984    1.44178   -0.0965118
  1.53713   -0.729689    1.44083   -0.0950135
  1.52934   -0.828521    1.43988   -0.0932133
  1.52056   -0.927455    1.43896   -0.0911382
  1.51079   -1.02645     1.43806   -0.0888436
  ⋮                                          
 -0.2928    -4.2953     -0.890906  -1.2365   
 -0.336127  -4.36932    -0.90206   -0.994903 
 -0.380169  -4.43808    -0.91082   -0.757953 
 -0.42487   -4.50118    -0.917241  -0.527494 
 -0.470171  -4.55792    -0.921399  -0.305739 
 -0.516004  -4.60727    -0.923393  -0.0953066
 -0.562287  -4.64784    -0.923353   0.100777 
 -0.608925  -4.67788    -0.921437   0.279132 
 -0.655803  -4.69543    -0.917841   0.43618  
 -0.702786  -4.69842    -0.912796   0.5684   
 -0.749717  -4.68496    -0.906566   0.672696 
 -0.749717  -4.68496    -0.906566   0.672696 

[:, :, 99] =
  1.44856     -0.00655098   1.61348   0.1189   
  1.44802     -0.101894     1.61465   0.115022 
  1.44652     -0.197157     1.61578   0.111004 
  1.44407     -0.292271     1.61687   0.10673  
  1.44068     -0.387166     1.61792   0.102081 
  1.43633     -0.481767     1.61891   0.0969273
  1.43104     -0.575991     1.61985   0.0911307
  1.42481     -0.669751     1.62073   0.0845388
  1.41765     -0.762951     1.62154   0.0769865
  1.40956     -0.855486     1.62227   0.0682936
  1.40054     -0.947243     1.6229    0.0582652
  1.39061     -1.0381       1.62343   0.0466914
  1.37978     -1.12794      1.62383   0.0333476
  ⋮                                            
  0.0758638   -2.32197     -1.67238  -5.15896  
  0.0520279   -2.44329     -1.72332  -5.029    
  0.0270368   -2.553       -1.77295  -4.89516  
  0.00100669  -2.65108     -1.82121  -4.7572   
 -0.0259462   -2.73757     -1.86808  -4.61503  
 -0.0537064   -2.81257     -1.9135   -4.4687   
 -0.0821597   -2.87624     -1.95744  -4.31835  
 -0.111194    -2.92882     -1.99985  -4.16425  
 -0.1407      -2.97061     -2.04071  -4.00674  
 -0.170571    -3.00195     -2.07998  -3.84624  
 -0.200706    -3.02325     -2.11763  -3.68321  
 -0.200706    -3.02325     -2.11763  -3.68321  

[:, :, 100] =
  1.5965    -0.0454943  1.65341  -0.146253
  1.59555   -0.143585   1.65194  -0.145996
  1.59363   -0.241655   1.65048  -0.145787
  1.59072   -0.339692   1.64903  -0.145656
  1.58683   -0.437678   1.64757  -0.14564 
  1.58197   -0.535591   1.64611  -0.145782
  1.57612   -0.633401   1.64466  -0.146141
  1.5693    -0.73107    1.64319  -0.14679 
  1.5615    -0.828546   1.64172  -0.147819
  1.55273   -0.925769   1.64023  -0.149342
  1.54299   -1.02266    1.63873  -0.151495
  1.53228   -1.11913    1.6372   -0.15444 
  1.52061   -1.21507    1.63564  -0.158366
  ⋮                                       
 -0.56794    0.306936   9.71883  -3.89927 
 -0.564838   0.313998   9.68     -3.86727 
 -0.561651   0.323761   9.64148  -3.83682 
 -0.558354   0.336152   9.60326  -3.80783 
 -0.55492    0.351098   9.56532  -3.78021 
 -0.551323   0.36853    9.52765  -3.75389 
 -0.547541   0.388379   9.49024  -3.72881 
 -0.543548   0.410573   9.45307  -3.70492 
 -0.539322   0.435041   9.41614  -3.68217 
 -0.53484    0.461703   9.37942  -3.66057 
 -0.530081   0.490476   9.34292  -3.64011 
 -0.530081   0.490476   9.34292  -3.64011
In [563]:
Array(sol0)
Out[563]:
4×1002 Array{Float64,2}:
 1.5708   1.57031      1.56884     …  -0.657791  -0.599271  -0.599271
 0.0     -0.098       -0.195999        5.687      5.99077    5.99077 
 1.5708   1.5708       1.5708         -0.382641  -0.477199  -0.477199
 0.0     -2.82357e-8  -9.03539e-7     -9.27754   -9.60107   -9.60107
In [564]:
sta_upd
Out[564]:
4×100 Array{Float64,2}:
 -0.935062   -0.920888   -0.931893   …  -0.906286   -0.91313    -0.920797 
  3.26107     3.35532     3.28579        3.42947     3.39291     3.32635  
  0.0669749   0.0499842   0.0616876      0.0384713   0.0441814   0.0561756
 -6.73579    -6.83418    -6.76596       -6.93498    -6.88786    -6.83654
In [565]:
u0
Out[565]:
4-element Array{Float64,1}:
 1.5707963267948966
 0.0               
 1.5707963267948966
 0.0
In [566]:
sta_var
Out[566]:
4×100 Array{Float64,2}:
 -0.983223  -0.961109   -0.977893  …  -0.939877   -0.949909   -0.962598 
  2.97471    3.11677     3.01246       3.23074     3.17502     3.07818  
  0.116921   0.0915294   0.109341      0.0730302   0.0821021   0.0994217
 -6.39894   -6.55306    -6.44428      -6.70036    -6.63087    -6.5443
In [567]:
x
Out[567]:
4×2 Array{Float64,2}:
 -0.920797   -0.408831
  3.32635     5.62316 
  0.0561756  -0.803253
 -6.83654    -8.85428
In [207]:
tspan = (0.0,tfinal);  # Time span (limits). The actual time variable is automatically set by solve().
# prob = ODEProblem(double_pendulum,mean(sta_upd,dims=2),tspan,p);
prob = ODEProblem(double_pendulum,mean(sta_upd,dims=2),tspan,p);
sol = solve(prob,Vern7(),dt=dt,minstep=dt,dtmax=dt);

anim=plotSolution(sol0,sol,p)
gif(anim,fps=10)

┌ Info: Saved animation to 
│   fn = C:\Users\amira\Dropbox (MIT)\CBA\_Classes\4_Spring 2020\Machine Learning System Optimization\Project\tmp.gif
└ @ Plots C:\Users\amira\.julia\packages\Plots\cc8wh\src\animation.jl:98
Out[207]:

Pendulum structure estimation

In [274]:
using DataDrivenDiffEq
using ModelingToolkit
using DiffEqSensitivity
In [275]:
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 [276]:
plot(X[1,:])
plot!(X[2,:])
Out[276]:
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 [279]:
@variables u[1:2]

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

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

dudt = ODEFunction(sys)

estimator = ODEProblem(dudt, u0, tspan, p)
estimation = solve(estimator, Tsit5(), saveat = solution.t);
Xest=Array(estimation)
Out[281]:
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 [282]:
println(Ψ)

2 dimensional basis in ["u₁", "u₂"]
du₁ = u₂ * 0.9944866064389278
du₂ = u₂ * -0.08978215064914737 + sin(u₁) * -9.798766377092148
In [283]:
plot(X[1,:])
plot!(X[2,:])
plot!(Xest[1,:],linestyle = :dash)
plot!(Xest[2,:],linestyle = :dash)
Out[283]:
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
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 [ ]: