In [8]:
# import Pkg; Pkg.update
# import Pkg; Pkg.add("ParameterizedFunctions")
# Pkg.add("PyDSTool")
# Pkg.add("Dierckx")
# import Pkg; Pkg.add("ParameterizedFunctions")
using Dierckx
using ParameterizedFunctions
# using PyDSTool
using Distributions, Compat
using StatsBase
using MultivariateStats
using StaticArrays, BenchmarkTools
using DifferentialEquations
using LinearAlgebra
using Plots
# import Pkg; Pkg.add("ImageView")
using Images
# using TestImages, Colors
# import Pkg; Pkg.add("PyPlot")
using Random
# import Pkg; Pkg.add("ODE")
using ODE
rng = MersenneTwister(1234);

http://essg.mit.edu/ml2020

$$J= \sum_{n=1}^{N} \frac{1}{2} [y_n-x_n]^2+ \lambda_n^T [x_n-F(x_{n-1}:\alpha)]$$
$$\lambda_n=\Delta F |_{x_n} \lambda_{n+1} +(y_n-x_n) $$
$$x_{n+1}= F(x_n;\alpha)$$
$$\frac{\delta J}{\delta \alpha}= \sum_{n=1}^{N} \Delta F |_{x_n} \lambda_{n+1} $$
In [ ]:
$$ \dot{x}=A x $$$$ x(0)= x_0 $$$$ x(\delta t)= e^{A \delta t} x(0)$$

$e$ matrix exponential $$ \dot{y}=A y =Ae^x $$ $$ t= n\delta t $$ $$ x_n= e^{A \delta t} x(n-1)$$

In [ ]:
$$ \dot{x}= F(x;\alpha) \leftarrow nonlinear$$$$ \dot{x} + \delta \dot{x}=F(x)+ \frac{\delta F}{\delta x}\delta x $$$$ \delta \dot{x}=\frac{\delta F}{\delta x} \delta x \leftarrow linear $$$$ \delta \dot{x}=A \delta x $$
In [ ]:
$$Z_{n+1}=A Z_n $$$$ A= e^{\frac{\delta F}{\delta x} |_{x=0}}$$$$\Delta t=0.01 $$
In [ ]:

True System:

$$\dot{x}=F (x;\alpha)$$$$x(0)=x_0$$$$y_n=Hx_n+\xi$$

$H$ incidence matrix $$H x_n= \begin{pmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end {pmatrix} \begin{pmatrix} \theta_1 \\ \omega_1 \\ \theta_2 \\ \omega_2 \end {pmatrix}$$ $$J= \sum_{n=1}^{N} \frac{1}{2} [y_n-z_n]^2+ \lambda_n^T [z_n-Az_{n-1}]$$

In [ ]:
$$x_n=F(x_0)=F(x0+\delta x_0)$$$$x1+\delta x_1=F(x_0)+ \frac{\delta F}{\delta x_0}\delta x_0$$$$\delta x_1=\frac{\delta F}{\delta x_0}\delta x_0$$$$<\delta x_1 ,\delta x_1^T > = \frac{\delta F}{\delta x_0} $$$$<\delta x_0 ,\delta x_0^T > = \frac{\delta F}{\delta x_0}^T $$$$\Sigma_{11}=A \Sigma_{11} A^T$$
$$\Sigma_{n+1,n+1}=A_n \Sigma_{nn} A_n^T$$$$x_{n+1}=F(x_n)$$$$\delta x_{n+1}= \frac{\delta F}{\delta x_n}\delta x_n$$$$A=\frac{\delta F}{\delta x_n}$$$$A^{-1} \delta x_{n+1}=\delta x_n$$$$A_n^{-1} \delta x_{n+1} \delta x^T_nn A_n^T= <\delta x_n,\delta x_n^T $$$$A_n^{-1} \Sigma_{n+1 n+1}A_n^T= \Sigma_{n n}$$
$$A_n^{T} \Sigma_{n+1 n+1}A_n^T= \Sigma^{-1}_{n n}$$$$\Sigma^{-1}_{n n}= A_n^{T}\Sigma^{-1}_{n+1 n+1} $$

$\Sigma_{n n}$ information matrix

In [ ]:

Forward: $$\Sigma_{n+1,n+1}=A \Sigma_{n n}A_n^{T} $$ Backward: $$J_{nn}=A_n^{T}J_{n+1,n+1} A_n$$

$$J=(y_n-x_n)R^{-1}(y_n-x_n)$$$$J=(x-\bar{x})\Sigma^{-1}(x-\bar{x})$$$$x^TJx+H^Tx+C$$

State Estimation: $$x_n=x_n+\Sigma_{nn}(\Sigma_nn+R)^{-1}(y_n-x_n)$$ Hessian of J: $$\Sigma_{nn}=R^{-1}+\Sigma_{nn}^{-1}$$ $$J_{nn}=J_R+J_{nn}$$

In [ ]:
In [9]:
# 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[9]:
double_pendulum (generic function with 1 method)
In [ ]:
$$ \dot{x}=A x $$$$ x(0)= x_0 $$$$ x(\delta t)= e^{A \delta t} x(0)$$
$$p_1=(m_1+m_2)L_1^2$$$$p_2= m_2 L_2^2$$$$p_3=m_2^2 L_1$$$$p_4=(m_1+m_2) L_1$$$$p_5=m_2 L_2$$
$$Jac= \begin{pmatrix} p1 & p3 \\ p3 & p2 \end {pmatrix}^{-1} . \begin{pmatrix} -p4*g & 0 \\ 0 & -p5*g \end {pmatrix}$$$$ Jac= \begin{pmatrix} (m_1+m_2)L_1^2 & L_1m_2^2 \\ L_1m_2^2 & m_2L_2^2 \end {pmatrix}^{-1} .\begin{pmatrix} -(m_1+m_2) L_1g & 0 \\ 0 & -m_2L_2g \end {pmatrix} $$$$ A=\begin{pmatrix} 0 & 1 & 0 & 0 \\Jac[1,1] & 0 & Jac[1,2] & 0\\ 0 & 0 & 0 & 1 \\ Jac[2,1] & 0 & Jac[2,2] & 0 \end {pmatrix}$$
$$TL=e^{A \Delta t}$$
In [10]:
println("linearized simulation with uncertainty propagation")
function DoPeLDM( par,dt )
    # UNTITLED Summary of this function goes here
    # Detailed explanation goes here

    m1 = par[1]  #First mass
    m2 = par[3]  #Second mass
    l1 = par[2]  #First length
    l2 = par[4]  #Second length
    g  = par[5]  #meters/sec^2

    p1 = (m1+m2)*l1*l1
    p2 = m2*l2*l2
    p3 = m2*l1*m2 
    p4 = (m1+m2)*l1
    p5=m2*l2

    LHS = [p1 p3; p3 p2];
    RHS = -[p4*g 0; 0  p5*g];
    Jac = inv(LHS)*RHS;

    A  = [ 0 1 0 0;
      Jac[1,1] 0 Jac[1,2] 0;
      0 0 0 1;
      Jac[2,1] 0 Jac[2,2] 0];

    TL = exp(A*dt);
    return TL

end

linearized simulation with uncertainty propagation
Out[10]:
DoPeLDM (generic function with 1 method)
$$ \dot{y}=A y =Ae^x $$$$ t= n\Delta t $$$$ x_n= e^{A \Delta t} x_{(n-1)}$$
$$\Sigma_{n+1,n+1}=A \Sigma_{n n}A_n^{T} $$
In [11]:
function DoPeLDMForward(xl0,Cii,TLM,nsteps)
    xl=xl0
    solution=zeros(4,nsteps)
    for i in 1:nsteps
        xl  = TLM*xl
        Cii = TLM*Cii*TLM
        solution[1,i]=xl[1] # phi1  
        solution[2,i]=xl[2] # dtphi1
        solution[3,i]=xl[3] # phi2  
        solution[4,i]=xl[4] # dtphi2
    end
    return solution,Cii
end
# linSol,Cii= DoPeLDMForward(xl0,C00,TLM,length(sol.t))
Out[11]:
DoPeLDMForward (generic function with 1 method)
In [12]:
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[12]:
plotSolution (generic function with 1 method)
In [13]:
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 = 5;
dt = 0.01;

u0 = [pi/2+randn()*pi/40; 0;pi/2+randn()*pi/40; 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);
sol = 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. Note that t is not uniformly spaced.
In [14]:
obs=zeros(length(sol.t),2)
for i in 1:length(sol.t)
    obs[i,1]=sol[1,i]+randn()*pi/40;
    obs[i,2]=sol[3,i]+randn()*pi/50;
    obs[i,1]=sol[1,i];
    obs[i,2]=sol[3,i];
end
In [15]:
C00 = [(2/180*pi)^2 0 0 0;
        0 (2/180*pi)^2 0 0;
        0 0 (2/180*pi)^2 0;
        0 0 0 (2/180*pi)^2]
TLM= DoPeLDM(p,0.01)
Out[15]:
4×4 Array{Float64,2}:
  0.99902     0.00999673   0.00048984  1.63301e-6
 -0.195904    0.99902      0.097936    0.00048984
  0.00097968  3.26603e-6   0.99902     0.00999673
  0.195872    0.00097968  -0.195904    0.99902
In [16]:
xl0 = u0[1:4].+ pi ./20 .*randn(4,1)
Cii = C00
nsteps=length(sol.t)
linSol,Cii= DoPeLDMForward(xl0,Cii,TLM,nsteps)
println(linSol[:,end])
anim=plotSolution(sol,linSol,p)
gif(anim,fps=10)

[1.0250830120541512, 2.6948679779766556, 1.9758960037743245, 1.3045617750925476]

┌ 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[16]:
$$\Sigma_{nn}=R^{-1}+\Sigma_{nn}^{-1}$$$$J_{nn}=J_R+J_{nn}$$$$J_{nn}=A_n^{T}J_{n+1,n+1} A_n$$
In [17]:
function forwardBackward(maxIter,nsteps,xl0,TLM,C00)
    Cii=C00
    linSol=0
    for jj in 1:maxIter
#         println("It: $jj")
        #forward run
        xl=xl0

        linSol,Cii= DoPeLDMForward(xl0,Cii,TLM,nsteps)
        xl=linSol[:,end]

        #Backward Run
        R=C00
        iR = inv(R)
        #Cii=convert(Array{Float64},Cii)

        Jii = inv(Cii) +iR

        dxl = xl
        dxl[[2,4]] .= 0
        dxl[[1,3]] .= obs[end,:] - xl[[1,3]]

        for i in nsteps:-1:1
            dxl  = TLM' *(dxl);   
            Jii = TLM' *Jii*TLM;
        end

        xl0 = xl0 + dxl;
        # Cii = inv(Jii)
        if(det(Jii)!=0)
            Cii = inv(Jii)
        else
#             return linSol
        end

    end
    return linSol
end
linSol=forwardBackward(100,length(sol.t),xl0,TLM,C00)
Out[17]:
4×502 Array{Float64,2}:
 -1.0748    -1.06278   -1.04941   …  -0.633551  -0.651316  -0.669194
  1.13252    1.26989    1.40509      -1.76934   -1.78292   -1.79185 
 -0.737801  -0.7343    -0.731444     -1.42881   -1.41776   -1.40522 
  0.38278    0.317531   0.254147      1.02727    1.18035    1.32761
In [18]:
anim=plotSolution(sol,linSol,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[18]:
In [ ]: