# Chasing the power of optimal length increase

(c) 2021 Tom Röschinger. This work is licensed under a [Creative Commons Attribution License CC-BY 4.0](https://creativecommons.org/licenses/by/4.0/). All code contained herein is licensed under an [MIT license](https://opensource.org/licenses/MIT).

***

```julia
using CSV, DataFrames, Distributed, Dates, LinearAlgebra, Distributions, DelimitedFiles, SharedArrays, Plots
# Custom package
using Jevo, Jedi

# Set plotting style
Jedi.default_gr!()
```

    ┌ Info: Precompiling Jedi [b681c197-c997-42fd-b5bb-d7d7839f617e]
    └ @ Base loading.jl:1278
    WARNING: using StatsBase.ecdf in module Jedi conflicts with an existing identifier.
    ┌ Warning: Package Jedi does not have Measures in its dependencies:
    │ - If you have Jedi checked out for development and have
    │   added Measures as a dependency but haven't updated your primary
    │   environment's manifest file, try `Pkg.resolve()`.
    │ - Otherwise you may need to report an issue with Jedi
    └ Loading Measures into Jedi from project dependency, future warnings for Jedi are suppressed.

    Plots.GRBackend()

## 1. Full Gamma Dynamics

First, let's have a look at the full $\Gamma$ dynamics. Therefore, we use the Kimura substitution probability to compute a distribution. Let's write down the rates for a match turning into a mismatch, $u_+(\Gamma, l)$, and the rate for a mismatch turning into a match, $u_-(\Gamma, l)$,

\begin{align}
u_+(\Gamma) &= \left(1 - \frac{\Gamma}{\epsilon l}\right) \left[p(F(\Gamma + \epsilon, l) - F(\Gamma, l) +\frac{\kappa}{N}\right],\\
u_-(\Gamma) &= \frac{\Gamma}{\epsilon l}\frac{1}{1-n} \left[p(F(\Gamma - \epsilon, l) - F(\Gamma, l) +\frac{\kappa}{N}\right],
\end{align}

where $p(s)$ is the Kimura substitution probability. The prefactors determine the probability of a match/mismatch mutating, which depends on the current state of the population. The extra prefactor $\frac{1}{1-n}$ comes from the consideration that a mismatch can mutate to another mismatch, which is not detectable on the level of quantitative traits. A similar factor is absent from the rate turning a match into am mismatch, since every mutation on a match leads to a mismatch.

Then we use these rates to determine the probability distribution over the binding energies,

$$
Q(\Gamma, l) \propto \prod_{\Gamma^\prime=\epsilon}^\Gamma \frac{u_+(\Gamma-\epsilon, l)}{u_-(\Gamma, l)}.
$$

It might be more useful to compute the free fitness $\Psi$ instead, which is simply the log of the distribution,

$$
\Psi(\Gamma, l) \propto \sum_{\Gamma^\prime=\epsilon}^\Gamma \log\left[\frac{u_+(\Gamma-\epsilon, l)}{u_-(\Gamma, l)}\right].
$$

```julia
# Neutral Expectation
γ_0(n) = (n-1)/n

# Binding Threshold
γ_1(l, n, l_0) = γ_0(n) - l_0/l

# Binding probability
pb(γ, l, n, l_0, gap) = 1 / (1 + exp(gap * (l / l_0) * (γ - γ_1(l, n, l_0))))

# Fitness component for functional binding
F_b(γ, l, n, l_0, gap, f0) = f0 * pb(γ, l, n, l_0, gap)

# Fitness component of genomic constraint
F_c(l, l_0, fl) = - fl * l / l_0

# Total Fitness
F(γ, l, n, l_0, gap, f0, fl) = F_b(γ, l, n, l_0, gap, f0) + F_c(l, l_0, fl)

function kimura(s)
    if s^2 < 10^(-20)
        return 1
    elseif s < -10^7
        return 0
    else
        return s/(1 - exp(-s))
    end
end

# Substitution rate for trait changes
# (k, l) -> (k + 1, l)
up(γ, l, n, l_0, gap, f0, fl, κ) =  (1 - γ) * (kimura(F(γ + 1/l, l, n, l_0, gap, f0, fl)
        - F(γ, l, n, l_0, gap, f0, fl)) + κ )

# (k, l) -> (k - 1, l)
um(γ, l, n, l_0, gap, f0, fl, κ) = γ * (1 - (n-2)/(n-1)) * (kimura(F(γ - 1/l, l, n, l_0, gap, f0, fl)
        - F(γ, l, n, l_0, gap, f0, fl)) + κ )

# Free Fitness
function Ψ(l, n, l_0, gap, f0, fl, κ)
    # Start at neutral γ
    γ_i = floor(γ_0(n) * l) / l
    return collect(0:1/l:γ_i), push!(Float64[0], cumsum([log(up(γ - 1/l, l,  n, l_0, gap, f0, fl, κ) / um(γ, l,  n, l_0, gap, f0, fl, κ)) for γ in 1/l:1/l:γ_i])...)

end

# (k,l) -> (k+1,l+1)
γ_pp(γ, l) = min((γ + 1/l)/(1 + 1/l), 1)

# (k,l) -> (k,l+1)
γ_pm(γ, l) = max(γ /(1 + 1/l), 0)

# (k,l) -> (k,l-1)
γ_mp(γ, l) = min(γ/(1 - 1/l), 1)

# (k,l) -> (k-1,l-1)
γ_mm(γ, l) = max((γ - 1/l)/(1 - 1/l), 0)

# selection coeffcients for length increase and decrease
# (k,l) -> (k+1,l+1)
s_pp(γ, l, n, l_0, gap, f0, fl) =
    F(γ_pp(γ, l), l + 1, n, l_0, gap, f0, fl) -
    F(γ, l, n, l_0, gap, f0, fl)

# (k,l) -> (k,l+1)
s_pm(γ, l, n, l_0, gap, f0, fl) =
    F(γ_pm(γ, l), l + 1, n, l_0, gap, f0, fl) -
    F(γ, l, n, l_0, gap, f0, fl)

# (k,l) -> (k-1,l-1)
s_mm(γ, l, n, l_0, gap, f0, fl) =
 F(γ_mm(γ, l), l - 1, n, l_0, gap, f0, fl) -
  F(γ, l, n, l_0, gap, f0, fl)

# (k,l) -> (k,l-1)
s_mp(γ, l, n, l_0, gap, f0, fl) =
 F(γ_mp(γ, l), l - 1, n, l_0, gap, f0, fl) -
  F(γ, l, n, l_0, gap, f0, fl)
```

    s_mp (generic function with 1 method)

Let's compare this theoretical prediction with some numerics. Therefore we first determine some parameters.

```julia
gap = 10
l_0 = 20
f0 = 40l_0
n = 4
N = 100

# Iniate Fitness landscape
F_th = Jevo.num_fermi(n, l_0, gap, f0,0)
F_num = Jevo.num_fermi(n, l_0, gap, f0/2N,0)

emat = gap/l_0 * (ones(n, n) - Matrix{Float64}(I, n, n))
```

    4×4 Array{Float64,2}:
     0.0  0.5  0.5  0.5
     0.5  0.0  0.5  0.5
     0.5  0.5  0.0  0.5
     0.5  0.5  0.5  0.0

This is the classic simulation code for substitution dynamics with fixed lengths and driver mutations.

```julia
# Run one rep
function Run(N, l, emat, F, κ, l_0, gap, steps)
    E_arr = zeros(Float64, steps)
    pop = Jevo.mono_pop(N=N, l=l)
    Jevo.initiate!(pop, emat)
    for r in 1:steps
        Jevo.bp_substitution!(pop, emat, F)
        if rand() < κ/N
            Jevo.driver_mutation!(pop)
        end
        E_arr[r] = Jevo.get_energy(pop, emat)
    end
    return E_arr
end
```

    Run (generic function with 1 method)

A quick custom function to make a histogram, since we have discrete values and don't rely on binning.

```julia
function make_histogram(data)
    x = sort(unique(data))
    y = [sum(data .== t) for t in x]
    return x, y
end
```

    make_histogram (generic function with 1 method)

Let's run a bunch of driving rates for some lengths.

```julia
function run_batch(κ, first_data=5000, steps=2*10^5)
    x, y = [], []
    for l in 40:1:200
        E = Run(N, l, emat, F_num, κ, l_0, gap, steps)
        _x, _y = make_histogram(E[first_data:end])
        push!(x, _x)
        push!(y, _y)
    end
    return x, y
end
```

    run_batch (generic function with 3 methods)

```julia
κ_arr = collect(0:5:10)
x_fix_list = []
y_fix_list = []
for κ in κ_arr
    x, y = run_batch(κ)
    push!(x_fix_list, x)
    push!(y_fix_list, y)
    println("κ = $κ done.")
end
```

    κ = 0 done.
    κ = 5 done.
    κ = 10 done.

I think it is more convenient to store the results in a dictionary.

```julia
fix_dict = Dict([x=>(y1,y2) for (x, y1, y2) in zip(κ_arr, x_fix_list, y_fix_list)]);
```

A quick and dirty graphical check is to simply look at many distributions at the same time.

```julia
# Probability distribution (from free Fitness)
function Q(l, n, l_0, gap, f0, fl, κ)
    x, y = Ψ(l, n, l_0, gap, f0, fl, κ)
    return x, exp.(y)
end

```

    Q (generic function with 1 method)

```julia
function plot_all_dists(κ)
    p_list = []
    for (i, l) in zip(1:10:161, 40:10:200)
        x_t, theory = Q(l, n, l_0, gap, F_th.f0, F_th.fl, κ)
        x_t = x_t .* (l *gap/l_0)
        theory = theory/ sum(theory)
        x, y = fix_dict[κ]
        p = plot(x_t, theory, title=l, xlims=[minimum(x[i]), maximum(x[i])])
        scatter!(twinx(), x[i], y[i] ./ sum(y[i]), xlims=[minimum(x[i]), maximum(x[i])])#, xlims=[minimum(x[i]), maximum(x[i])])
        #plot!(twinx(), x_t, Jevo.fitness.(x_t, l, F), color="orange", xlims=[minimum(x[i]), maximum(x[i])], ylims=([0.9, 1.] .* F.f0 .+ F.fl/l_0 *l))
        push!(p_list, p)
    end
    return plot(p_list..., size=(1000, 1000))
end
```

    plot_all_dists (generic function with 1 method)

```julia
plot_all_dists(0)
```

```julia
plot_all_dists(10)
```

Now we want to use these distributions to compute the optimal length.

```julia
function kimura(s::Real)
    if abs(s) <= 10^-20
        return 1 / N
    elseif s < -10^6
        return 0
    else
        return s / (1 - exp(-s))
    end
end
```

    kimura (generic function with 3 methods)

```julia
function p_l_th(κ, n, l_0, gap, f0, fl, l_range=collect(40:200))
    vp = []
    vm = []
    for l in l_range
        x, y = Q(l, n, l_0, gap, f0, fl, κ)
        y = y/sum(y)
        s_mm_p = [s_pp(γ, l, n, l_0, gap, f0, fl) for γ in x]
        s_m_p = [s_pm(γ, l, n, l_0, gap, f0, fl) for γ in x]
        s_mm_m = [s_mm(γ, l, n, l_0, gap, f0, fl) for γ in x]
        s_m_m = [s_mp(γ, l, n, l_0, gap, f0, fl) for γ in x]
        v_mm_p = [(n-1)/n * kimura(s) for s in s_mm_p]
        v_m_p = [1/n * kimura(s) for s in s_m_p]
        v_mm_m = [ γ * kimura(s) for (s, γ) in zip(s_mm_m, x)]
        v_m_m = [(1 - γ)  * kimura(s) for (s, γ) in zip(s_m_m, x)]
        push!(vp, sum((v_m_p .+ v_mm_p) .* y))
        push!(vm, sum((v_mm_m .+ v_m_m) .* y))
    end
    return vp, vm, push!([0.01], (cumsum([log(vp[i]/vm[i+1]) for i in 1:length(vp)-1]).+0.01)...)
end
```

    p_l_th (generic function with 2 methods)

```julia
function p_l_sim(κ)
    vp = []
    vm = []
    x, y = fix_dict[κ]
    for (i, l) in enumerate(40:200)
        _x = x[i]
        _y = y[i]
        _y = _y/sum(_y)
        s_mm_p = [Jevo.fitness((k+gap/l_0), l+1, F) - Jevo.fitness(k, l, F) for k in _x]
        s_m_p = [Jevo.fitness(k, l+1, F) - Jevo.fitness(k, l, F) for k in _x]
        s_mm_m = [ k > 0 ? Jevo.fitness(k-gap/l_0, l-1, F) - Jevo.fitness(k, l, F) : -10^8 for k in _x]
        s_m_m = [Jevo.fitness(k , l-1, F) - Jevo.fitness(k, l, F) for k in _x]
        v_mm_p = [(n-1)/n * kimura(s, N) for s in s_mm_p]
        v_m_p = [1/n * kimura(s, N) for s in s_m_p]
        v_mm_m = [ k * l_0/gap /l * kimura(s, N) for (s, k) in zip(s_mm_m, _x)]
        v_m_m = [(1 - (k * l_0/gap /l))  * kimura(s, N) for (s, k) in zip(s_m_m, _x)]
        push!(vp, sum((v_m_p .+ v_mm_p) .* _y))
        push!(vm, sum((v_mm_m .+ v_m_m) .* _y))
    end
    return push!([0.01], (cumsum([log(vp[i]/vm[i+1]) for i in 1:length(vp)-1]).+0.01)...)
end
```

    p_l_sim (generic function with 1 method)

```julia
x_κ = collect(0:0.5:20)
l_opt_sim = []
l_opt_theory = []
for κ in x_κ
    push!(l_opt_theory, argmax(p_l_th(κ, n, l_0, gap, F_th.f0, F_th.fl)) + 39)
    #push!(l_opt_sim, argmax(p_l_sim(κ)) + 39)
end

plot(x_κ, l_opt_theory)
#plot!(x_κ, l_opt_sim)
l_opt_theory
```

    41-element Array{Any,1}:
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40
      ⋮
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40
     40

```julia
x = κ_arr[1:end] .+ 1
l_max_theory = argmax.(p_l_list[1:end]).+39
l_max_plot = plot(x, l_max_theory, label="semi numeric")
plot!(l_max_plot, x, x .^ (1/3) /(21)^(1/3)*l_max_theory[end], linestyle=:dash, label="^1/3")
plot!(l_max_plot, x, x .^ (1/2) * l_max_theory[1], linestyle=:dash, label="^1/2")
plot!(l_max_plot, xlabel="1+κ", ylabel="l_opt")
plot!(l_max_plot, xxx.+1, yy)

```

```julia
x_max100 = []
for (l, κ) in zip(l_max_theory, collect(0:0.5:10))
    E = Run(N, l, emat, F, κ, l_0, gap, 10^5)
    x, y = make_histogram(E[5000:end])
    push!(x_max100, x[argmax(y)]/l*2)
end

plot(log.(collect(0:0.5:10) .+1), log.(x_max100))
plot!(log.(1 .+ collect(0:0.5:10)), log.((collect(0:0.5:10).+1).^(1/6) .* x_max100[end]/10^(1/6)))
plot!(log.(1 .+ collect(0:0.5:10)), log.((collect(0:0.5:10).+1).^(1) .* x_max100[1]))
```

```julia
plot(collect(0:0.5:10) .+1, x_max100)
plot!(1 .+ collect(0:0.5:10), (collect(0:0.5:10).+1).^(1/6) .* x_max100[1])
plot!(1 .+ collect(0:0.5:10), (collect(0:0.5:10).+1).^(1) .* x_max100[1])
```

```julia
plot(log.(x), log.(l_max_theory), linewidth=2, title="log-log")
plot!(log.(x), log.(x .^ (1/2) * l_max_theory[1]), linestyle=:dash, label="^1/2")
plot!(log.(x), log.(x .^ (1/3) /(21)^(1/3)*l_max_theory[end]), linestyle=:dash, label="^1/3")
```

### Dynamic L

```julia
K(x, x_arr, y_arr, h) = 1/sum(y_arr)/h * sum([exp(-1/2*((x - x_arr[i])/h)^2) * y_arr[i] for i in eachindex(x_arr)])
```

    K (generic function with 1 method)

```julia
df = CSV.read("../outputs/2021_02_06_results.csv", DataFrame)
p_list = []
lmax = []
for _df in groupby(df, "kappa")
    y_plot = [K(x, _df.l, _df.p_l, 1) for x in _df.l]
    push!(p_list, plot(_df.l, _df.p_l))
    push!(lmax, _df.l[argmax(y_plot)])
end
plot(p_list...)
```

```julia

```

```julia

```

```julia
scatter!(l_max_plot, κ_arr[1:end-1] .+ 1, lmax[1:end-1], xlabel="1+κ", ylabel="l_opt")
```

```julia

```

```julia

```


# Circuitos magnéticamente acoplados
__UNLZ - Facultad de Ingeniería__
__Electrotecnia__
__Alumno:__ Daniel Antonio Lorenzo

1. Se dice que dos bobinas están acopladas mutuamente si el flujo magnético $\phi$ que emana de una de ellas pasa por la otra. La inductancia mutua entre las dos bobinas está dada por

$$ M = k \sqrt{L_1 L_2} $$

donde $k$ es el coeficiente de acoplamiento $0<k<1$.

La inductancia mutua es la capacidad de un inductor de inducir una tensión en un inductor cercano, medida en henrys (H).

2. Si $v_1$ e $i_1$ son la tensión y la corriente en la bobina 1, mientras que $v_2$ e $i_2$ son la tensión y la corriente en la bobina 2, entonces

$$ v_1 = L_1 \frac{di_1}{dt} + M \frac{di_2}{dt} \qquad \mbox{y} \qquad v_2 = L_2 \frac{di_2}{dt} + M \frac{di_1}{dt} $$

Así, la tensión inducida en una bobina acoplada consta de la tensión autoinducida y la tensión mutua.

3. La polaridad de la tensión inducida mutuamente se expresa en diagramas mediante la convención de las marcas de polaridad

4. La energía almacenada en las dos bobinas acopladas es

$$ \frac{1}{2} L_1 i_1^2 + \frac{1}{2} L_2 i_2^2 \pm Mi_1i_2 $$

5. Un transformador es un dispositivo de cuatro terminales que contiene dos o más bobinas acopladas magnéticamente. Se emplea para modificar el nivel de corriente, tensión o impedancia en un circuito.

6. Las bobinas de un transformador lineal (o acoplado con holgura) están devanadas magéticamente en un material lineal. Este transformador puede reemplazarse por una red T o $\Pi$ equivalente para efectos de análisis.

7. Un transformador ideal (o con núcleo de hierro) es un transformador sin pérdidas $(R_1=R_2=R_3)=0$ con coeficiente de acoplamiento unitario $(k=1)$ e inductancias infinitas $(L_1, \, L_2, \,M \rightarrow \infty)$.

8. En un transformador ideal

$$ V_2 = n V_1 \qquad I_2 = \frac{I_1}{n} \qquad S_1 = S_2 \qquad Z_R = \frac{Z_L}{n^2} $$

donde $n=N_2/N_1$, es la relación de vueltas. $N_1$ es el número de vueltas del devanado primario y $N_2$ el número de vueltas del devanado secundario. El transformador aumenta la tensión primaria cuando $n>1$, la reduce cuando $n<1$ o sirve como dispositivo acoplador cuando $n=1$.

9. Un autotransformador es un transformador con un mismo devanado común a los circuitos primario y secundario.

10. Los transformadores son necesarios en todas las etapas de los sistemas de distribución de potencia. Las tensiones trifásicas pueden aumentarse o reducirse mediante transformadores trifásicos.

11. Usos importantes de los transformadores en aplicaciones electrónicas son como dispositivos de aislamiento eléctrico y como dispositivos de acoplamiento de impedancias.

## Ejemplo 13.1
Calcule las corrientes fasoriales $I_1$ e $I_2$ del circuito de la figura

### Solución
En relación con la bobina 1, la LTK da como resultado

$$ -12 + (-j4 + j5)I_1 - j3 I_2 = 0 $$

o sea
$$ jI_1 - j3I_2 = 12 \tag{1} $$

En la bobina 2, la LTK da por resultado

$$ -j3I_1 + (12 + j6)I_2 = 0 $$

o sea
$$ I_1 = \frac{(12 + j6)I_2}{j3} = (2 - j4)I_2 \tag{2} $$

Al sustituir (2) en (1) se obtiene

$$ j(2 - j4)I_2 - j3I_2 = 12 $$

$$ (j2 + 4)I_2 - j3I_2 = 12 $$

$$ I_2(4 - j) = 12 $$

$$ I_2 = \frac{12}{4-j} = 2,91 \angle 14,04^\circ \, \mathrm{A} \tag{3} $$

Con base en las ecuaciones (2) y (3)

$$ I_1 = (2-j4)I_2 = (4,472 \angle -63,43^\circ) \cdot (2,91 \angle 14,04^\circ) $$

$$ I_1 = 13,02 \angle -49,40^\circ \, \mathrm{A} $$

### Usando sympy

```python
import sympy as sp
```

```python
import math, cmath
```

```python
I1, I2 = sp.symbols('I1 I2')
Eq1 = sp.Eq(-12 + (-4j + 5j)*I1 - 3j*I2 , 0)
Eq2 = sp.Eq(-3j*I1 + (12 + 6j)*I2 , 0)
```

```python
Sol = sp.solve([Eq1,Eq2],[I1,I2])
```

```python
print('I1 = (%.2f < %.2f°) A'%( abs(Sol[I1]) , math.degrees( cmath.phase(Sol[I1] ) ) ) )
print('I2 = (%.2f < %.2f°) A'%( abs(Sol[I2]) , math.degrees( cmath.phase(Sol[I2] ) ) ) )
```

    I1 = (13.02 < -49.40°) A
    I2 = (2.91 < 14.04°) A

### Por regla de Cramer, (usando numpy)

$$\left[
\begin{array}{cc}
1j & -3j \\
-3j & 12+6j
\end{array}
\right]
\left[
\begin{array}{c}
I_1 \\
I_2
\end{array}
\right]
=
\left[
\begin{array}{c}
12 \\
0
\end{array}
\right]$$

```python
import numpy as np
```

```python
M = np.array([[1j , -3j],[-3j , 12+6j] ])
M1 = np.array([[12 , -3j],[0 , 12+6j] ])
M2 = np.array([[1j , 12],[-3j , 0] ])
```

```python
Delta = np.linalg.det(M)
Delta1 = np.linalg.det(M1)
Delta2 = np.linalg.det(M2)
I1 = Delta1/Delta
I2 = Delta2/Delta
```

$$\Delta = \left|
\begin{array}{cc}
1j & -3j \\
-3j & 12+6j
\end{array}
\right| $$

```python
print('Delta = {:.2f}'.format(Delta))
```

    Delta = 3.00+12.00j

$$\Delta_1 = \left|
\begin{array}{cc}
12 & -3j \\
0 & 12+6j
\end{array}
\right| $$

```python
print('Delta1 = {:.2f}'.format(Delta1))
```

    Delta1 = 144.00+72.00j

$$\Delta_2 = \left|
\begin{array}{cc}
1j & 12 \\
-3j & 0
\end{array}
\right| $$

```python
print('Delta2 = {:.2f}'.format(Delta2))
```

    Delta2 = 0.00+36.00j

$$ I_1 = \frac{\Delta_1}{\Delta} \qquad ; \qquad I_2 = \frac{\Delta_2}{\Delta} $$

```python
print('I1 = (%.2f < %.2f°) A'%(abs(I1) , math.degrees( cmath.phase(I1) )))
print('I2 = (%.2f < %.2f°) A'%(abs(I2) , math.degrees( cmath.phase(I2) )))
```

    I1 = (13.02 < -49.40°) A
    I2 = (2.91 < 14.04°) A

### Otro método

$$\left[
\begin{array}{cc}
1j & -3j \\
-3j & 12+6j
\end{array}
\right]
\left[
\begin{array}{c}
I_1 \\
I_2
\end{array}
\right]
=
\left[
\begin{array}{c}
12 \\
0
\end{array}
\right]$$

$$ A I = B $$

Entonces
$$ I = A^{-1} B $$

```python
Ap = np.linalg.inv(M)
B = np.array([[12] , [0]])
I = np.dot(Ap , B)
```

```python
print('I1 = (%.2f < %.2f) A'%(abs(I[0]) , math.degrees( cmath.phase(I[0]) ) ))
print('I2 = (%.2f < %.2f) A'%(abs(I[1]) , math.degrees( cmath.phase(I[1]) ) ))
```

    I1 = (13.02 < -49.40) A
    I2 = (2.91 < 14.04) A

```python
%reset -s -f
```

## Problema de práctica 13.1
Determine la tensión $V_0$ en el circuito de la figura.

### Solución

```python
import numpy as np
```

```python
import math, cmath
```

```python
# Datos:
Vs = cmath.rect(100 , np.deg2rad(45) ) # V
A = np.array([ [4+8j , -1j],[-1j , 10+5j] ])
B = np.array([ [Vs],[0] ])
```

```python
I = np.dot(np.linalg.inv(A) , B)
```

```python
print('I1 = (%.2f < %.2f) A'%(abs(I[0]) , np.rad2deg( cmath.phase(I[0]) ) ))
print('I2 = (%.2f < %.2f) A'%(abs(I[1]) , np.rad2deg( cmath.phase(I[1]) ) ))
```

    I1 = (11.18 < -17.86) A
    I2 = (1.00 < 45.57) A

$$ V = IR $$

```python
I1 = I[0] ; I2 = I[1]
R2 = 10 # Ohm
Vo = -(I2*R2)  # (caida de tensión)
```

```python
print('Vo = (%.2f < %.2f° V)'%(abs(Vo) , np.rad2deg( cmath.phase(Vo) ) ))
```

    Vo = (10.00 < 45.57° V)

```python
A1 = np.array([ [Vs , -1j],[0 , 10+5j] ])
A2 = np.array([ [4+8j , Vs],[-1j , 0] ])
Delta = np.linalg.det(A)
Delta1 = np.linalg.det(A1)
Delta2 = np.linalg.det(A2)
I1 = Delta1/Delta
I2 = Delta2/Delta
```

```python
print('I1 = (%.2f < %.2f) A'%(abs(I1) , np.rad2deg( cmath.phase(I1) ) ))
print('I2 = (%.2f < %.2f) A'%(abs(I2) , np.rad2deg( cmath.phase(I2) ) ))
```

    I1 = (11.18 < -17.86) A
    I2 = (1.00 < 45.57) A

```python
Vo = -(I2*R2) # (caida de tensión)
```

```python
print('Vo = (%.2f < %.2f° V)'%(abs(Vo) , np.rad2deg( cmath.phase(Vo) ) ))
```

    Vo = (10.00 < -134.43° V)

```python
%reset -s -f
```

## Ejemplo 13.2
Calcule las corrientes de malla en el circuito de la figura.

### Solución
La clave para analizar un circuito magnéticamente acoplado es conocer la polaridad de la tensión mutua. Se debe aplicar la regla del punto. En la figura, supóngase que la bobina 1 es aquella cuya reactancia es de $6 \, \Omega$,  la bobina 2 aquella cuya reactancia es de $8 \, \Omega$. Para deducir la polaridad de la tensión mutua en la bobina 1 debida a la corriente $I_2$, se  observa que $I_2$ sale de la terminal marcada de la bobina 2. Puesto que se está aplicando la LTK en el sentido de las manecillas del reloj, esto implica que la tensión mutua es negativa, es decir $-j2I_2$.

Así, en cuanto al lazo 1 de la figura, la LTK da como resultado

$$ -100 + I_1 (4 - j3 + j6) - j6I_2 - j2 I_2 = 0 $$

o
$$ (4+j3)I_1 - j8I_2 = 100 \tag{1} $$

En consecuencia, en relación con la malla 2 de la figura, la LTK produce

$$ -2jI_1 -j6I_1 + (j6 + j8 + j2 \times 2 + 5)I_2 = 0 $$

o
$$ -j8I_1 + (5 + j18)I_2 = 0 \tag{2} $$

Al colocar las ecuaciones (1) y (2) en forma matricial se obtiene

$$\left[
\begin{array}{cc}
4+j3 & -j8 \\
-j8 & 5+j18
\end{array}
\right]
\left[
\begin{array}{c}
I_1 \\
I_2
\end{array}
\right]
=
\left[
\begin{array}{c}
100 \\
0
\end{array}
\right]$$

Alternativamnete, podría ser mejor deducir la tensión mutua redibujando la porción pertinente del circuito, como se muestra en la fig., donde resulta claro que la tensión mutua es $V_1 = -2jI_2$.

Los determinantes son

$$\begin{align}
\Delta &= \left|
\begin{array}{cc}
4+j3 & -j8 \\
-j8 & 5+j18
\end{array}
\right| = 30+j87 \\
\Delta_1 &= \left|
\begin{array}{cc}
100 & -j8 \\
0 & 5+j18
\end{array}
\right| = 500+j1800 \\
\Delta_2 &= \left|
\begin{array}{cc}
4+j3 & 100 \\
-j8 & 0
\end{array}
\right| = 0+j800
\end{align}$$

```python
import numpy as np
```

```python
M = np.array([ [4+3j , -8j],[-8j , 5+18j] ])
M1 = np.array([ [100 , -8j],[0 , 5+18j] ])
M2 = np.array([ [4+3j , 100],[-8j , 0] ])

Delta = np.linalg.det(M)
Delta1 = np.linalg.det(M1)
Delta2 = np.linalg.det(M2)
```

```python
print('Delta = {:.0f}'.format(Delta))
print('Delta1 = {:.0f}'.format(Delta1))
print('Delta2 = {:.0f}'.format(Delta2))
```

    Delta = 30+87j
    Delta1 = 500+1800j
    Delta2 = 0+800j

$$ I_1 = \frac{\Delta_1}{\Delta} = 20,30 \angle 3,50^\circ \, \mathrm{A} $$

$$ I_2 = \frac{\Delta_2}{\Delta} = 8,693 \angle 19,03^\circ \, \mathrm{A} $$

```python
I1 = Delta1/Delta
I2 = Delta2/Delta
```

```python
print('I1 = (%.2f < %.2f°) A'%(abs(I1) , np.rad2deg( np.angle(I1) ) ))
print('I2 = (%.3f < %.2f°) A'%(abs(I2) , np.rad2deg( np.angle(I2) ) ))
```

    I1 = (20.30 < 3.50°) A
    I2 = (8.693 < 19.03°) A

```python
%reset -s -f
```

## Ejemplo 13.3
Considere el circuito de la figura. Determine el coeficiente de acoplamiento. Calcule la energía almacenada en los inductores acoplados en el momento $t=1 \, s$ si $v = 60 \cos (4t + 30^\circ) \, \mathrm{V}$

### Solución
El coeficiente de acoplamiento es

$$ k = \frac{M}{\sqrt{L_1 L_2}} = \frac{2,5}{\sqrt{5 \cdot 4}} = 0,559 $$

lo que indica que los inductores están acoplados estrechamente. Para hallar la energía almacenada, se debe calcular la corriente. Para encontrar la corriente, debe obtenerse el equivalente del circuito en el dominio de la frecuencia.

$$\begin{array}{rcl}
60 \cos (4t + 30^\circ) & \Rightarrow & 60 \angle 30^\circ, \quad \omega = 4 \, \mathrm{rad/s} \\
5 \, \mathrm{H} & \Rightarrow & j \omega L_1 = j20 \, \Omega \\
2,5 \, \mathrm{H} & \Rightarrow & j \omega M = j10 \, \Omega \\
4 \, \mathrm{H} & \Rightarrow & j \omega L_2 = j16 \, \Omega \\
\displaystyle \frac{1}{16} \, \mathrm{F} & \Rightarrow & \displaystyle \frac{1}{j \omega C} = -j4 \, \Omega
\end{array}$$

Ahora se aplica el análisis de mallas. En cuanto al lazo 1,

$$ (10 + j20) I_1 + j10 I_2 = 60 \angle 30^\circ $$

En cuanto al lazo 2

$$ j10 I_1 + (j16 - j4)I_2 = 0 $$

o sea
$$ j10I_1 + j12 I_2 = 0 $$

```python
import cmath
import numpy as np
```

```python
# Datos
w = 4 # rad/s
R1 = 10 # Ohm
L1 = 5 # H
L2 = 4 # H
M = 2.5 # H
C1 = 1/16 # F
Vs = cmath.rect( 60 , np.deg2rad(30) )
```

```python
XL1 = complex(0 , w*L1)
XL2 = complex(0 , w*L2)
XC1 = complex(0 , -1/(w*C1) )
Xm = complex(0 , w*M)
```

```python
A = np.array([[R1+XL1 , Xm],[Xm , XL2 + XC1]])
B = np.array([ [Vs] , [0] ])
```

```python
I = np.dot( np.linalg.inv(A) , B )
```

```python
print('I1 = (%.3f < %.2f°) A'%(abs(I[0]) , np.rad2deg( np.angle(I[0]) ) ) )
print('I2 = (%.3f < %.2f°) A'%(abs(I[1]) , np.rad2deg( np.angle(I[1]) ) ) )
```

    I1 = (3.905 < -19.40°) A
    I2 = (3.254 < 160.60°) A

__Simulación en qucs:__

Descargar archivo qucs

En el dominio temporal

$\begin{array}{l}
i_1 = 3,905 \cos (4t - 19,4^\circ) \\
i_2 = 3,254 \cos (4t + 160,6^\circ)
\end{array}$

En el momento  $t = 1 \, s \quad \rightarrow \quad 4t = 4 \, \mathrm{rad} = 229,2^\circ$

```python
def i1(t):
    return abs(I[0])*np.cos(4*t + np.angle(I[0]))

def i2(t):
    return abs(I[1])*np.cos(4*t + np.angle(I[1]))
```

```python
time = 1 # s
print('i1 = %.3f A'%i1(time))
print('i2 = %.3f A'%i2(time))
```

    i1 = -3.389 A
    i2 = 2.824 A

La energía total almacenada en los dos inductores acoplados es

$$ \omega = \frac{1}{2} L_1 i_1^2 + \frac{1}{2} L_2 i_2^2 + M i_1 i_2 $$

```python
W = 1/2 * L1 * i1(time)**2 + 1/2 * L2 * i2(time)**2 + M * i1(time) * i2(time)
```

```python
print('W = %.2f J'%W)
```

    W = 20.74 J

```python
%reset -s -f
```

## Problema de práctica 13.3
En referencia al circuito de la figura, determine el coeficiente de acoplamiento y la energía almacenada en los inductores acoplados en $t = 1,5 \, \mathrm{s}$.

### Solución

El coeficiente de acoplamiento es

$$ k = \frac{M}{\sqrt{L_1 L_2}} = \frac{1}{\sqrt{2 \cdot 1}} = 0,707 $$

```python
import cmath
import numpy as np
```

```python
k = 1/np.sqrt(2*1)
```

```python
print('k = %.4f'%k)
```

    k = 0.7071

En este caso $k>0,5$, entonces se dice que las bobinas están acopladas estrechamente.

Para encontrar la corriente, debe obtenerse el equivalente del circuito en el dominio de la frecuencia.

$$\begin{array}{rcl}
40 \cos 2t & \Rightarrow & 40 \angle 0^\circ , \quad \omega = 2 \, \mathrm{rad/s} \\
2 \, \mathrm{H} & \Rightarrow & j \omega L_1 = j4 \, \Omega \\
1 \, \mathrm{H} & \Rightarrow & j \omega M = j2 \, \Omega\\
1 \, \mathrm{H} & \Rightarrow & j \omega L_2 = j2 \, \Omega\\
\displaystyle \frac{1}{8} \, \mathrm{F} & \Rightarrow & \displaystyle \frac{1}{j \omega C} = -j4 \, \Omega
\end{array}$$

```python
# Datos:
Vs = 40 # V
w = 2 # rad/s
R1 = 4 # Ohm
R2 = 2 # Ohm
L1 = 2 # H
L2 = 1 # H
M = 1 # H
C1 = 1/8 # F
```

```python
XL1 = complex(0 , w*L1)
XL2 = complex(0 , w*L2)
XM = complex(0 , w*M)
XC1 = complex(0 , -1/(w*C1))
```

```python
print('XL1 = {:.1f} Ohm'.format(XL1))
print('XL2 = {:.1f} Ohm'.format(XL2))
print('XM = {:.1f} Ohm'.format(XM))
print('XC1 = {:.1f} Ohm'.format(XC1))
```

    XL1 = 0.0+4.0j Ohm
    XL2 = 0.0+2.0j Ohm
    XM = 0.0+2.0j Ohm
    XC1 = 0.0-4.0j Ohm

```python
A = np.array([ [R1 + XC1 + XL1 , XM], [XM , XL2 + R2 ] ])
B = np.array([ [Vs],[0] ])
```

```python
I =  np.dot(np.linalg.inv(A) , B)
```

```python
print('I1 = (%.3f < %.2f°) A'%(abs(I[0]) , np.rad2deg( cmath.phase(I[0]) ) ))
print('I2 = (%.3f < %.2f°) A'%(abs(I[1]) , np.rad2deg( cmath.phase(I[1]) ) ))
```

    I1 = (7.845 < 11.31°) A
    I2 = (5.547 < -123.69°) A

__Simulación en qucs:__

Descargar archivo qucs

En el dominio temporal

$\begin{array}{l}
i_1 = 7,845 \cos (2t + 11,31^\circ) \\
i_2 = 5,547 \cos (2t - 123,69^\circ)
\end{array}$

```python
def i1(t):
    return abs(I[0])*np.cos(w*t + np.angle(I[0]))

def i2(t):
    return abs(I[1])*np.cos(w*t + np.angle(I[1]))
```

En el momento $t = 1,5 \, s$

```python
time = 1.5 # s
print('i1(1.5s) = %.3f A'%i1(time))
print('i2(1.5s) = %.3f A'%i2(time))
```

    i1(1.5s) = -7.832 A
    i2(1.5s) = 3.697 A

La energía total almacenada en los dos inductores acoplados es

$$ W = \frac{1}{2} L_1 i_1^2 + \frac{1}{2} L_2^2 i_2^2 + M i_1 i_2 $$

```python
W = (1/2)*L1*i1(time)**2 + (1/2)*L2*i2(time)**2 + M*i1(time) * i2(time)
```

```python
print('W = %.2f J'%W)
```

    W = 39.22 J

```python
%reset -s -f
```


[Table of Contents](http://nbviewer.ipython.org/github/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/table_of_contents.ipynb)

# Designing Nonlinear Kalman Filters

```python
#format the book
%matplotlib inline
from __future__ import division, print_function
import book_format
book_format.load_style()
```

<style>
@import url('http://fonts.googleapis.com/css?family=Source+Code+Pro');
@import url('http://fonts.googleapis.com/css?family=Vollkorn');
@import url('http://fonts.googleapis.com/css?family=Arimo');

    div.cell{
        width: 900px;
        margin-left: 0% !important;
        margin-right: auto;
    }
    div.text_cell code {
        background: transparent;
        color: #000000;
        font-weight: 600;
        font-size: 11pt;
        font-style: bold;
        font-family:  'Source Code Pro', Consolas, monocco, monospace;
   }
    h1 {
        font-family: 'Open sans',verdana,arial,sans-serif;
	}

    div.input_area {
        background: #F6F6F9;
        border: 1px solid #586e75;
    }

    .text_cell_render h1 {
        font-weight: 200;
        font-size: 30pt;
        line-height: 100%;
        color:#c76c0c;
        margin-bottom: 0.5em;
        margin-top: 1em;
        display: block;
        white-space: wrap;
    }
    h2 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }
    .text_cell_render h2 {
        font-weight: 200;
        font-size: 16pt;
        font-style: italic;
        line-height: 100%;
        color:#c76c0c;
        margin-bottom: 0.5em;
        margin-top: 1.5em;
        display: inline;
        white-space: wrap;
    }
    h3 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }
    .text_cell_render h3 {
        font-weight: 200;
        font-size: 14pt;
        line-height: 100%;
        color:#d77c0c;
        margin-bottom: 0.5em;
        margin-top: 2em;
        display: block;
        white-space: nowrap;
    }
    h4 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }
    .text_cell_render h4 {
        font-weight: 100;
        font-size: 14pt;
        color:#d77c0c;
        margin-bottom: 0.5em;
        margin-top: 0.5em;
        display: block;
        white-space: nowrap;
    }
    h5 {
        font-family: 'Open sans',verdana,arial,sans-serif;
    }
    .text_cell_render h5 {
        font-weight: 200;
        font-style: normal;
        color: #1d3b84;
        font-size: 16pt;
        margin-bottom: 0em;
        margin-top: 0.5em;
        display: block;
        white-space: nowrap;
    }
    div.text_cell_render{
        font-family: 'Arimo',verdana,arial,sans-serif;
        line-height: 125%;
        font-size: 120%;
        text-align:justify;
        text-justify:inter-word;
    }
    div.output_subarea.output_text.output_pyout {
        overflow-x: auto;
        overflow-y: scroll;
        max-height: 50000px;
    }
    div.output_subarea.output_stream.output_stdout.output_text {
        overflow-x: auto;
        overflow-y: scroll;
        max-height: 50000px;
    }
    div.output_wrapper{
        margin-top:0.2em;
        margin-bottom:0.2em;
}

    code{
      font-size: 70%;
    }
    .rendered_html code{
    background-color: transparent;
    }
    ul{
        margin: 2em;
    }
    ul li{
        padding-left: 0.5em;
        margin-bottom: 0.5em;
        margin-top: 0.5em;
    }
    ul li li{
        padding-left: 0.2em;
        margin-bottom: 0.2em;
        margin-top: 0.2em;
    }
    ol{
        margin: 2em;
    }
    ol li{
        padding-left: 0.5em;
        margin-bottom: 0.5em;
        margin-top: 0.5em;
    }
    ul li{
        padding-left: 0.5em;
        margin-bottom: 0.5em;
        margin-top: 0.2em;
    }
    a:link{
       font-weight: bold;
       color:#447adb;
    }
    a:visited{
       font-weight: bold;
       color: #1d3b84;
    }
    a:hover{
       font-weight: bold;
       color: #1d3b84;
    }
    a:focus{
       font-weight: bold;
       color:#447adb;
    }
    a:active{
       font-weight: bold;
       color:#447adb;
    }
    .rendered_html :link {
       text-decoration: underline;
    }
    .rendered_html :hover {
       text-decoration: none;
    }
    .rendered_html :visited {
      text-decoration: none;
    }
    .rendered_html :focus {
      text-decoration: none;
    }
    .rendered_html :active {
      text-decoration: none;
    }
    .warning{
        color: rgb( 240, 20, 20 )
    }
    hr {
      color: #f3f3f3;
      background-color: #f3f3f3;
      height: 1px;
    }
    blockquote{
      display:block;
      background: #fcfcfc;
      border-left: 5px solid #c76c0c;
      font-family: 'Open sans',verdana,arial,sans-serif;
      width:680px;
      padding: 10px 10px 10px 10px;
      text-align:justify;
      text-justify:inter-word;
      }
      blockquote p {
        margin-bottom: 0;
        line-height: 125%;
        font-size: 100%;
      }
</style>

## Introduction

** Author's note: I was initially planning to have a design nonlinear chapter that compares various approaches. This may or may not happen, but for now this chapter has no useful content and I suggest not reading it. **

We see that the Kalman filter reasonably tracks the ball. However, as already explained, this is a silly example; we can predict trajectories in a vacuum with arbitrary precision; using a Kalman filter in this example is a needless complication.

### Kalman Filter with Air Drag

I will dispense with the step 1, step 2, type approach and proceed in a more natural style that you would use in a non-toy engineering problem. We have already developed a Kalman filter that does excellently at tracking a ball in a vacuum, but that does not incorporate the effects of air drag into the model. We know that the process model is implemented with $\textbf{F}$, so we will turn our attention to that immediately.

Notionally, the computation that $\textbf{F}$ computes is

$$x' = Fx$$

With no air drag, we had

$$
\mathbf{F} = \begin{bmatrix}
1 & \Delta t & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 \\
0 & 0 & 1 & \Delta t & \frac{1}{2}{\Delta t}^2 \\
0 & 0 & 0 & 1 & \Delta t \\
0 & 0 & 0 & 0 & 1
\end{bmatrix}
$$

which corresponds to the equations

$$
\begin{aligned}
x &= x + v_x \Delta t \\
v_x &= v_x \\
\\
y &= y + v_y \Delta t + \frac{a_y}{2} {\Delta t}^2 \\
v_y &= v_y + a_y \Delta t \\
a_y &= a_y
\end{aligned}
$$

From the section above we know that our new Euler equations must be

$$
\begin{aligned}
x &= x + v_x \Delta t \\
v_x &= v_x \\
\\
y &= y + v_y \Delta t + \frac{a_y}{2} {\Delta t}^2 \\
v_y &= v_y + a_y \Delta t \\
a_y &= a_y
\end{aligned}
$$

## Realistic 2D Position Sensors

The position sensor in the last example are not very realistic. In general there is no 'raw' sensor that provides (x,y) coordinates. We have GPS, but GPS already uses a Kalman filter to create a filtered output; we should not be able to improve the signal by passing it through another Kalman filter unless we incorporate additional sensors to provide additional information. We will tackle that problem later.

Consider the following set up. In an open field we put two transmitters at a known location, each transmitting a signal that we can detect. We process the signal and determine  how far we are from that signal, with some noise. First, let's look at a visual depiction of that.

```python
import matplotlib.pyplot as plt

circle1=plt.Circle((-4, 0), 5, color='#004080',
                   fill=False, linewidth=20, alpha=.7)
circle2=plt.Circle((4, 0), 5, color='#E24A33',
                   fill=False, linewidth=5, alpha=.7)

fig = plt.gcf()
ax = fig.gca()

plt.axis('equal')
plt.xlim((-10, 10))
plt.ylim((-10, 10))

plt.plot ([-4, 0], [0, 3], c='#004080')
plt.plot ([4, 0], [0, 3], c='#E24A33')
plt.text(-4, -.5, "A", fontsize=16, horizontalalignment='center')
plt.text(4, -.5, "B", fontsize=16, horizontalalignment='center')

ax.add_artist(circle1)
ax.add_artist(circle2)
plt.show()
```

Here I have attempted to show transmitter A, drawn in red, at (-4,0) and a second one B, drawn in blue, at (4,0). The red and blue circles show the range from the transmitters to the robot, with the width illustrating the effect of the $1\sigma$ angular error for each transmitter. Here I have given the blue transmitter more error than the red one. The most probable position for the robot is where the two circles intersect, which I have depicted with the red and blue lines. You will object that we have two intersections, not one, but we will see how we deal with that when we design the measurement function.

This is a very common sensor set up. Aircraft still use this system to navigate, where it is called DME (Distance Measuring Equipment). Today GPS is a much more common navigation system, but I have worked on an aircraft where we integrated sensors like this into our filter along with the GPS, INS, altimeters, etc. We will tackle what is called *multi-sensor fusion* later; for now we will just address this simple configuration.

The first step is to design our state variables. We will assume that the robot is traveling in a straight direction with constant velocity. This is unlikely to be true for a long period of time, but is acceptable for short periods of time. This does not differ from the previous problem - we will want to track the values for the robot's position and velocity. Hence,

$$\mathbf{x} =
\begin{bmatrix}x\\v_x\\y\\v_y\end{bmatrix}$$

The next step is to design the state transition function. This also will be the same as the previous problem, so without further ado,

$$
\mathbf{x}' = \begin{bmatrix}1& \Delta t& 0& 0\\0& 1& 0& 0\\0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{bmatrix}\mathbf{x}$$

The next step is to design the control inputs. We have none, so we set ${\mathbf{B}}=0$.

The next step is to design the measurement function $\mathbf{z} = \mathbf{Hx}$. We can model the measurement using the Pythagorean theorem.

$$
z_a = \sqrt{(x-x_A)^2 + (y-y_A)^2} + v_a\\[1em]
z_b = \sqrt{(x-x_B])^2 + (y-y_B)^2} + v_b
$$

where $v_a$ and $v_b$ are white noise.

We see an immediate problem. The Kalman filter is designed for linear equations, and this is obviously nonlinear. In the next chapters we will look at several ways to handle nonlinear problems in a robust way, but for now we will do something simpler. If we know the approximate position of the robot than we can linearize these equations around that point. I could develop the generalized mathematics for this technique now, but instead let me just present the worked example to give context to that development.

Instead of computing $\mathbf{H}$ we will compute the partial derivative of $\mathbf{H}$ with respect to the robot's position $\mathbf{x}$. You are probably familiar with the concept of partial derivative, but if not, it just means how $\mathbf{H}$ changes with respect to the robot's position. It is computed as the partial derivative of $\mathbf{H}$ as follows:

$$\frac{\partial \mathbf{h}}{\partial \mathbf{x}} =
\begin{bmatrix}
\frac{\partial h_1}{\partial x_1} & \frac{\partial h_1}{\partial x_2} &\dots \\
\frac{\partial h_2}{\partial x_1} & \frac{\partial h_2}{\partial x_2} &\dots \\
\vdots & \vdots
\end{bmatrix}
$$

Let's work the first partial derivative. We want to find

$$\frac{\partial }{\partial x} \sqrt{(x-x_A)^2 + (y-y_A)^2}
$$

Which we compute as

$$
\begin{aligned}
\frac{\partial h_1}{\partial x} &= ((x-x_A)^2 + (y-y_A)^2))^\frac{1}{2} \\
&= \frac{1}{2}\times 2(x-x_a)\times ((x-x_A)^2 + (y-y_A)^2))^{-\frac{1}{2}} \\
&= \frac{x_r - x_A}{\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}}
\end{aligned}
$$

We continue this computation for the partial derivatives of the two distance equations with respect to $x$, $y$, $dx$ and $dy$, yielding

$$\frac{\partial\mathbf{h}}{\partial\mathbf{x}}=
\begin{bmatrix}
\frac{x_r - x_A}{\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}} & 0 &
\frac{y_r - y_A}{\sqrt{(x_r-x_A)^2 + (y_r-y_A)^2}} & 0 \\
\frac{x_r - x_B}{\sqrt{(x_r-x_B)^2 + (y_r-y_B)^2}} & 0 &
\frac{y_r - y_B}{\sqrt{(x_r-x_B)^2 + (y_r-y_B)^2}} & 0 \\
\end{bmatrix}
$$

That is pretty painful, and these are very simple equations. Computing the Jacobian can be extremely difficult or even impossible for more complicated systems. However, there is an easy way to get Python to do the work for you by using the SymPy module [1]. SymPy is a Python library for symbolic mathematics. The full scope of its abilities are beyond this book, but it can perform algebra, integrate and differentiate equations, find solutions to differential equations, and much more. We will use it to compute our Jacobian!

First, a simple example. We will import SymPy, initialize its pretty print functionality (which will print equations using LaTeX). We will then declare a symbol for NumPy to use.

```python
import sympy
from sympy import init_printing
init_printing(use_latex='png')

phi, x = sympy.symbols('\phi, x')
phi
```

Notice how we use a latex expression for the symbol `phi`. This is not necessary, but if you do it will render as LaTeX when output. Now let's do some math. What is the derivative of $\sqrt{\phi}$?

```python
sympy.diff('sqrt(phi)')
```

We can factor equations.

```python
sympy.factor('phi**3 -phi**2 + phi - 1')
```

SymPy has a remarkable list of features, and as much as I enjoy exercising its features we cannot cover them all here. Instead, let's compute our Jacobian.

```python
from sympy import symbols, Matrix
phi = symbols('\phi')
phi

x, y, xa, xb, ya, yb, dx, dy = symbols('x y x_a x_b y_a y_b dx dy')

H = Matrix([[sympy.sqrt((x-xa)**2 + (y-ya)**2)],
            [sympy.sqrt((x-xb)**2 + (y-yb)**2)]])

state = Matrix([x, dx, y, dy])
H.jacobian(state)
```

In a nutshell, the entry (0,0) contains the difference between the x coordinate of the robot and transmitter A's x coordinate divided by the distance between the robot and A. (2,0) contains the same, except for the y coordinates of the robot and transmitters. The bottom row contains the same computations, except for transmitter B. The 0 entries account for the velocity components of the state variables; naturally the range does not provide us with velocity.

The values in this matrix change as the robot's position changes, so this is no longer a constant; we will have to recompute it for every time step of the filter.

If you look at this you may realize that this is just a computation of x/dist and y/dist, so we can switch this to a trigonometic form with no loss of generality:

$$\frac{\partial\mathbf{h}}{\partial\mathbf{x}}=
\begin{bmatrix}
-\cos{\theta_A} & 0 & -\sin{\theta_A} & 0 \\
-\cos{\theta_B} & 0 & -\sin{\theta_B} & 0
\end{bmatrix}
$$

However, this raises a huge problem. We are no longer computing $\mathbf{H}$, but $\Delta\mathbf{H}$, the change of $\mathbf{H}$. If we passed this into our Kalman filter without altering the rest of the design the output would be nonsense. Recall, for example, that we multiply $\mathbf{Hx}$ to generate the measurements that would result from the given estimate of $\mathbf{x}$ But now that $\mathbf{H}$ is linearized around our position it contains the *change* in the measurement function.

We are forced, therefore, to use the *change* in $\mathbf{x}$ for our state variables. So we have to go back and redesign our state variables.

>Please note this is a completely normal occurrence in designing Kalman filters. The textbooks present examples like this as *fait accompli*, as if it is trivially obvious that the state variables needed to be velocities, not positions. Perhaps once you do enough of these problems it would be trivially obvious, but at that point why are you reading a textbook? I find myself reading through a presentation multiple times, trying to figure out why they made a choice, finally to realize that it is because of the consequences of something on the next page. My presentation is longer, but it reflects what actually happens when you design a filter. You make what seem reasonable design choices, and as you move forward you discover properties that require you to recast your earlier steps. As a result, I am going to somewhat abandon my **step 1**, **step 2**, etc.,  approach, since so many real problems are not quite that straightforward.

If our state variables contain the velocities of the robot and not the position then how do we track where the robot is? We can't. Kalman filters that are linearized in this fashion use what is called a *nominal trajectory* - i.e. you assume a position and track direction, and then apply the changes in velocity and acceleration to compute the changes in that trajectory. How could it be otherwise? Recall the graphic showing the intersection of the two range circles - there are two areas of intersection. Think of what this would look like if the two transmitters were very close to each other - the intersections would be two very long crescent shapes. This Kalman filter, as designed, has no way of knowing your true position from only distance measurements to the transmitters. Perhaps your mind is already leaping to ways of working around this problem. If so, stay engaged, as later sections and chapters will provide you with these techniques. Presenting the full solution all at once leads to more confusion than insight, in my opinion.

So let's redesign our *state transition function*. We are assuming constant velocity and no acceleration, giving state equations of
$$
\dot{x}' = \dot{x} \\
\ddot{x}' = 0 \\
\dot{y}' = \dot{y} \\
\dot{y}' = 0$$

This gives us the the *state transition function* of

$$
\mathbf{F} = \begin{bmatrix}0 &1 & 0& 0\\0& 0& 0& 0\\0& 0& 0& 1\\ 0& 0& 0& 0\end{bmatrix}$$

A final complication comes from the measurements that we pass in. $\mathbf{Hx}$ is now computing the *change* in the measurement from our nominal position, so the measurement that we pass in needs to be not the range to A and B, but the *change* in range from our measured range to our nominal position.

There is a lot here to take in, so let's work through the code bit by bit. First we will define a function to compute $\frac{\partial\mathbf{h}}{\partial\mathbf{x}}$ for each time step.

```python
from math import sin, cos, atan2

def H_of(pos, pos_A, pos_B):
    """ Given the position of our object at 'pos' in 2D, and two
    transmitters A and B at positions 'pos_A' and 'pos_B', return
    the partial derivative of H
    """

    theta_a = atan2(pos_a[1] - pos[1], pos_a[0] - pos[0])
    theta_b = atan2(pos_b[1] - pos[1], pos_b[0] - pos[0])

    return np.array([[0, -cos(theta_a), 0, -sin(theta_a)],
                     [0, -cos(theta_b), 0, -sin(theta_b)]])
```

Now we need to create our simulated sensor.

```python
from numpy.random import randn

class DMESensor(object):
    def __init__(self, pos_a, pos_b, noise_factor=1.0):
        self.A = pos_a
        self.B = pos_b
        self.noise_factor = noise_factor

    def range_of(self, pos):
        """ returns tuple containing noisy range data to A and B
        given a position 'pos'
        """

        ra = math.sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
        rb = math.sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)

        return (ra + randn()*self.noise_factor,
                rb + randn()*self.noise_factor)
```

Finally, we are ready for the Kalman filter code. I will position the transmitters at x=-100 and 100, both with y=-20. This gives me enough space to get good triangulation from both as the robot moves. I will start the robot at (0,0) and move by (1,1) each time step.

```python
import math
from filterpy.kalman import KalmanFilter
import numpy as np
import book_plots as bp

pos_a = (100, -20)
pos_b = (-100, -20)

f1 = KalmanFilter(dim_x=4, dim_z=2)

f1.F = np.array ([[0, 1, 0, 0],
                  [0, 0, 0, 0],
                  [0, 0, 0, 1],
                  [0, 0, 0, 0]], dtype=float)

f1.R *= 1.
f1.Q *= .1

f1.x = np.array([[1, 0, 1, 0]], dtype=float).T
f1.P = np.eye(4) * 5.

# initialize storage and other variables for the run
count = 30
xs, ys = [], []
pxs, pys = [], []

# create the simulated sensor
d = DMESensor(pos_a, pos_b, noise_factor=3.)

# pos will contain our nominal position since the filter does not
# maintain position.
pos = [0, 0]

for i in range(count):
    # move (1,1) each step, so just use i
    pos = [i, i]

    # compute the difference in range between the nominal track
    # and measured ranges
    ra,rb = d.range_of(pos)
    rx,ry = d.range_of((pos[0] + f1.x[0, 0], pos[1] + f1.x[2, 0]))
    z = np.array([[ra - rx], [rb - ry]])

    # compute linearized H for this time step
    f1.H = H_of (pos, pos_a, pos_b)

    # store stuff so we can plot it later
    xs.append(f1.x[0, 0]+i)
    ys.append(f1.x[2, 0]+i)
    pxs.append(pos[0])
    pys.append(pos[1])

    # perform the Kalman filter steps
    f1.predict()
    f1.update(z)

bp.plot_filter(xs, ys)
bp.plot_track(pxs, pys)
plt.legend(loc=2)
plt.show()
```

## Linearizing the Kalman Filter

Now that we have seen an example of linearizing the Kalman filter we are in a position to better understand the math.

We start by assuming some function $\mathbf f$

## Example: A falling Ball

**author's note: ignore this section for now. **

In the **Designing Kalman Filters** chapter I first considered tracking a ball in a vacuum, and then in the atmosphere. The Kalman filter performed very well for vacuum, but diverged from the ball's path in the atmosphere. Let us look at the output; to avoid littering this chapter with code from that chapter I have placed it all in the file `ekf_internal.py'.

```python
import ekf_internal
ekf_internal.plot_ball()
```

We can artificially force the Kalman filter to track the ball by making $Q$ large. That would cause the filter to mistrust its prediction, and scale the kalman gain $K$ to strongly favor the measurments. However, this is not a valid approach. If the Kalman filter is correctly predicting the process we should not 'lie' to the filter by telling it there are process errors that do not exist. We may get away with that for some problems, in some conditions, but in general the Kalman filter's performance will be substandard.

Recall from the **Designing Kalman Filters** chapter that the acceleration is

$$a_x = (0.0039 + \frac{0.0058}{1+\exp{[(v-35)/5]}})*v*v_x \\
a_y = (0.0039 + \frac{0.0058}{1+\exp{[(v-35)/5]}})*v*v_y- g
$$

These equations will be *very* unpleasant to work with while we develop this subject, so for now I will retreat to a simpler one dimensional problem using this simplified equation for acceleration that does not take the nonlinearity of the drag coefficient into account:

$$\begin{aligned}
\ddot{y} &= \frac{0.0034ge^{-y/20000}\dot{y}^2}{2\beta} - g \\
\ddot{x} &= \frac{0.0034ge^{-x/20000}\dot{x}^2}{2\beta}
\end{aligned}$$

Here $\beta$ is the ballistic coefficient, where a high number indicates a low drag.

This is still nonlinear, so we need to linearize this equation at the current state point. If our state is position and velocity, we need an equation for some arbitrarily small change in $\mathbf{x}$, like so:

$$ \begin{bmatrix}\Delta \dot{x} \\ \Delta \ddot{x} \\ \Delta \dot{y} \\ \Delta \ddot{y}\end{bmatrix} =
\large\begin{bmatrix}
\frac{\partial \dot{x}}{\partial x} &
\frac{\partial \dot{x}}{\partial \dot{x}} &
\frac{\partial \dot{x}}{\partial y} &
\frac{\partial \dot{x}}{\partial \dot{y}} \\
\frac{\partial \ddot{x}}{\partial x} &
\frac{\partial \ddot{x}}{\partial \dot{x}}&
\frac{\partial \ddot{x}}{\partial y}&
\frac{\partial \dot{x}}{\partial \dot{y}}\\
\frac{\partial \dot{y}}{\partial x} &
\frac{\partial \dot{y}}{\partial \dot{x}} &
\frac{\partial \dot{y}}{\partial y} &
\frac{\partial \dot{y}}{\partial \dot{y}} \\
\frac{\partial \ddot{y}}{\partial x} &
\frac{\partial \ddot{y}}{\partial \dot{x}}&
\frac{\partial \ddot{y}}{\partial y}&
\frac{\partial \dot{y}}{\partial \dot{y}}
\end{bmatrix}\normalsize
\begin{bmatrix}\Delta x \\ \Delta \dot{x} \\ \Delta \dot{y} \\ \Delta \ddot{y}\end{bmatrix}$$

The equations do not contain both an x and a y, so any partial derivative with both in it must be equal to zero. We also know that $\large\frac{\partial \dot{x}}{\partial x}\normalsize = 0$ and that  $\large\frac{\partial \dot{x}}{\partial \dot{x}}\normalsize = 1$, so our matrix ends up being

$$\mathbf{F} = \begin{bmatrix}0&1&0&0 \\
\frac{0.0034e^{-x/22000}\dot{x}^2g}{44000\beta}&0&0&0
\end{bmatrix}$$

$$\begin{aligned}\ddot{x} &= -\frac{1}{2}C_d\rho A \dot{x}\\
\ddot{y} &= -\frac{1}{2}C_d\rho A \dot{y}-g\end{aligned}$$

```python
from sympy.abc import *
from sympy import *

init_printing(pretty_print=True, use_latex='mathjax')

x1 = (0.0034*g*exp(-x/22000)*((x)**2))/(2*b) - g

x2 = (a*g*exp(-x/c)*(Derivative(x)**2))/(2*b) - g

#pprint(x1)
#pprint(Derivative(x)*Derivative(x,n=2))
#pprint(diff(x2, x))
```

** orphan text
This approach has many issues. First, of course, is the fact that the linearization does not produce an exact answer. More importantly, we are not linearizing the actual path, but our filter's estimation of the path. We linearize the estimation because it is statistically likely to be correct; but of course it is not required to be. So if the filter's output is bad that will cause us to linearize an incorrect estimate, which will almost certainly lead to an even worse estimate. In these cases the filter will quickly diverge. This is where the 'black art' of Kalman filter comes in. We are trying to linearize an estimate, and there is no guarantee that the filter will be stable. A vast amount of the literature on Kalman filters is devoted to this problem. Another issue is that we need to linearize the system using analytic methods. It may be difficult or impossible to find an analytic solution to some problems. In other cases we may be able to find the linearization, but the computation is very expensive. **

## References

[1] http://sympy.org


```python
import numpy as np
import scipy as sc
import random as rand
from sklearn import preprocessing, linear_model
import matplotlib.pyplot as plt

from core.controllers import PDController
from core.dynamics import LinearSystemDynamics, ConfigurationDynamics

from koopman_core.controllers import OpenLoopController, MPCController,BilinearFBLinController, PerturbedController, LinearLiftedController
from koopman_core.dynamics import LinearLiftedDynamics, BilinearLiftedDynamics
from koopman_core.learning import Edmd, BilinearEdmd
from koopman_core.basis_functions import PolySineBasis
from koopman_core.learning.utils import differentiate_vec
from koopman_core.systems import PlanarQuadrotorForceInput

class QuadrotorPdOutput(ConfigurationDynamics):
    def __init__(self, dynamics, xd, t_d, n, m):
        ConfigurationDynamics.__init__(self, dynamics, 1)
        self.xd = xd
        self.t_d = t_d
        self.xd_dot = differentiate_vec(self.xd, self.t_d)
        self.n = n
        self.m = m

    def proportional(self, x, t):
        q, q_dot = x[:int(n/2)], x[int(n/2):]
        return self.y(q) - self.y_d(t)

    def derivative(self, x, t):
        q, q_dot = x[:int(n/2)], x[int(n/2):]
        return self.dydq(q)@q_dot - self.y_d_dot(t)

    def y(self, q):
        return q

    def dydq(self, q):
        return np.eye(int(self.n/2))

    def d2ydq2(self, q):
        return np.zeros((int(self.n/2), int(self.n/2), int(self.n/2)))

    def y_d(self, t):
        return self.desired_state_(t)[:int(self.n/2)]

    def y_d_dot(self, t):
        return self.desired_state_(t)[int(self.n/2):]

    def y_d_ddot(self, t):
        return self.desired_state_dot_(t)[int(self.n/2):]

    def desired_state_(self, t):
        return [np.interp(t, self.t_d.flatten(),self.xd[:,ii].flatten()) for ii in range(self.xd.shape[1])]

    def desired_state_dot_(self, t):
        return [np.interp(t, self.t_d.flatten(),self.xd_dot[:,ii].flatten()) for ii in range(self.xd_dot.shape[1])]

class QuadrotorTrajectoryOutput(ConfigurationDynamics):
    def __init__(self, bilinear_dynamics, y_d, y_d_dot, y_d_ddot, dt, z_d=None, z_d_dot=None, z_d_ddot=None, C_h=None):
        ConfigurationDynamics.__init__(self, bilinear_dynamics, 2)
        self.bilinear_dynamics = bilinear_dynamics
        self.ref = y_d
        self.ref_dot = y_d_dot
        self.ref_ddot = y_d_ddot

        self.ref_z = z_d
        self.ref_dot_z = z_d_dot
        self.ref_ddot_z = z_d_ddot

        self.C_h = C_h
        self.dt = dt
        self.t_d = self.dt * np.arange(0, self.ref.shape[1])

    def eval_z(self, x, t):
        z = self.bilinear_dynamics.phi_fun(x.reshape(1,-1)).squeeze()
        return z - self.z_d(t)

    def y(self, q):
        return q

    def dydq(self, q):
        return np.array([[1, 0], [0, 1]])

    def d2ydq2(self, q):
        return np.zeros((1, 2, 2))

    def y_d(self, t):
        return self.interpolate_ref_(self.ref, t)

    def y_d_dot(self, t):
        return self.interpolate_ref_(self.ref_dot, t)

    def y_d_ddot(self, t):
        return self.interpolate_ref_(self.ref_ddot, t)

    def z_d(self, t):
        return self.interpolate_ref_(self.ref_z, t)

    def z_d_dot(self, t):
        return self.interpolate_ref_(self.ref_dot_z, t)

    def z_d_ddot(self, t):
        return self.interpolate_ref_(self.ref_ddot_z, t)

    def interpolate_ref_(self, ref, t):
        return np.array([np.interp(t, self.t_d, ref[ii, :]) for ii in range(ref.shape[0])])
```

## Planar Quadrotor Example

Consider a planar quadrotor with states $\mathbf{x} = [y \, z \, \theta \, \dot{y} \, \dot{z} \, \dot{\theta}]^T$ and continuous-time dynamics

\begin{equation}
    \begin{bmatrix} \ddot{y} \\ \ddot{z} \\ \ddot{\theta} \end{bmatrix}
    = \begin{bmatrix}
    0\\-g\\0
    \end{bmatrix} +
    \begin{bmatrix}
    -\frac{1}{m}\text{sin}\theta & -\frac{1}{m}\text{sin}\theta\\
    \frac{1}{m}\text{cos}\theta & \frac{1}{m}\text{cos}\theta\\
    -\frac{l_{arm}}{I_{xx}} & \frac{l_{arm}}{I_{xx}}
    \end{bmatrix}
    \begin{bmatrix}
    T_1 \\ T_2
    \end{bmatrix}
\end{equation}

where $y,z$ describe the position of the vehicle in a fixed reference frame, $\theta$ is the orientation of the vehicle,
$T_1, T_2$ are the thrust from each of the propellers, $g$ is the gravitational acceleration, $m$ is the vehicle mass,
$l_{arm}$ is the distance from the vehicle's center of mass to the center of the propeller, and $I_{xx}$ is the inertia
around the x-axis.

```python
# Cart pole system parameters
mass = 2.
inertia = 1.
prop_arm = 0.2
gravity = 9.81
quadrotor = PlanarQuadrotorForceInput(mass, inertia, prop_arm, g=gravity)

# Linearized system specification:
n, m = 6, 2                                                         # Number of states, number of control inputs
A_nom = np.array([[0., 0., 0., 1., 0., 0.],                         # Linearization of the true system around the origin
                  [0., 0., 0., 0., 1., 0.],
                  [0., 0., 0., 0., 0., 1.],
                  [0., 0., -gravity, 0., 0., 0.],
                  [0., 0., 0., 0., 0., 0.],
                  [0., 0., 0., 0., 0., 0.]])
B_nom = np.array([[0., 0.],                                         # Linearization of the true system around the origin
                  [0., 0.],
                  [0., 0.],
                  [0., 0.],
                  [1./mass, 1./mass],
                  [-prop_arm/inertia, prop_arm/inertia]])

hover_thrust = mass*gravity/m
```

### Collect data for learning

To collect data, a nominal controller is designed with LQR on the dynamics's linearization around hover. However, any
controller can be used and the method does not require the knowledge of model's linearization. In addition, a
exploratory white noise is added to the controller to ensure that the data is sufficiently excited. Note that the system
is underactuated and that trajectory optimization is necessary to control the position of the vehicle. We use a
simplified trajectory generator based on a model predictive controller for the linearized dynamics. More careful design
of the desired trajectory may be necessary for more demanding applications and this is readily compatible with our method.

```python
q_dc, r_dc = 1e2, 1                                                 # State and actuation penalty values, data collection
Q_dc = q_dc * np.identity(n)                                        # State penalty matrix, data collection
R_dc = r_dc*np.identity(m)                                          # Actuation penalty matrix, data collection
P_dc = sc.linalg.solve_continuous_are(A_nom, B_nom, Q_dc, R_dc)     # Algebraic Ricatti equation solution, data collection
K_dc = np.linalg.inv(R_dc)@B_nom.T@P_dc                             # LQR feedback gain matrix, data collection
K_dc_p = K_dc[:,:int(n/2)]                                          # Proportional control gains, data collection
K_dc_d = K_dc[:,int(n/2):]                                          # Derivative control gains, data collection
nominal_sys = LinearLiftedDynamics(A=A_nom, B=B_nom, C=np.eye(n), basis = lambda x: x)

# Data collection parameters:
dt = 1.0e-2                                                         # Time step length
traj_length_dc = 2.                                                 # Trajectory length, data collection
n_pred_dc = int(traj_length_dc/dt)                                  # Number of time steps, data collection
t_eval = dt * np.arange(n_pred_dc + 1)                              # Simulation time points
n_traj_dc = 50                                                      # Number of trajectories to execute, data collection
noise_var = 1.                                                      # Exploration noise to perturb controller, data collection

xmax = np.array([2, 2, np.pi/3, 2.,2.,2.])                          # State constraints, trajectory generation
xmin = -xmax
umax = np.array([50., 50.]) - hover_thrust                          # Actuation constraint, trajectory generation
umin = np.array([0., 0.]) - hover_thrust
x0_max = np.array([xmax[0], xmax[1], xmax[2], 1., 1., 1.])          # Initial value limits
Q_trajgen = sc.sparse.diags([0,0,0,0,0,0])                          # State penalty matrix, trajectory generation
QN_trajgen = sc.sparse.diags([5e1,5e1,5e1,1e1,1e1,1e1])             # Final state penalty matrix, trajectory generation
R_trajgen = sc.sparse.eye(m)                                        # Actuation penalty matrix, trajectory generation
sub_sample_rate = 5                                                 # Rate to subsample data for training
model_fname = 'examples/planar_quad_models'                         # Path to save learned models
n_cols = 10                                                         # Number of columns in training data plot
```

```python
xd = np.empty((n_traj_dc, n_pred_dc + 1, n))
xs = np.empty((n_traj_dc, n_pred_dc + 1, n))
us = np.empty((n_traj_dc, n_pred_dc, m))

plt.figure(figsize=(12, 12 * n_traj_dc / (n_cols ** 2)))
for ii in range(n_traj_dc):
    x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                QN_trajgen, set_pt_dc)
    mpc_trajgen.eval(x0, 0)
    xd[ii, :, :] = mpc_trajgen.parse_result().T
    while abs(x0[0]) < 1.25 or np.any(np.isnan(xd[ii, :, :])):
        x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                    QN_trajgen, set_pt_dc)
        mpc_trajgen.eval(x0, 0)
        xd[ii, :, :] = mpc_trajgen.parse_result().T

    output = QuadrotorPdOutput(quadrotor, xd[ii, :, :], t_eval, n, m)
    pd_controller = PDController(output, K_dc_p, K_dc_d)
    perturbed_pd_controller = PerturbedController(quadrotor, pd_controller, noise_var, const_offset=hover_thrust)
    xs[ii, :, :], us[ii, :, :] = quadrotor.simulate(x0, perturbed_pd_controller, t_eval)

    plt.subplot(int(np.ceil(n_traj_dc / n_cols)), n_cols, ii + 1)
    plt.plot(t_eval, xs[ii, :, 0], 'b', label='$y$')
    plt.plot(t_eval, xs[ii, :, 1], 'g', label='$z$')
    plt.plot(t_eval, xs[ii, :, 2], 'r', label='$\\theta$')
    plt.plot(t_eval, xd[ii, :, 0], '--b', label='$y_d$')
    plt.plot(t_eval, xd[ii, :, 1], '--g', label='$z_d$')
    plt.plot(t_eval, xd[ii, :, 2], '--r', label='$\\theta_d$')

plt.suptitle(
    'Training data \nx-axis: time (sec), y-axis: state value, $x$ - blue, $xd$ - dotted blue, $\\theta$ - red, $\\theta_d$ - dotted red',
    y=0.94)
plt.show()
```

### Learn a linear model with dynamic mode decomposition (DMD)

To compare our method with existing techniques, we first learn a linear state space model from data. This is dubbed
dynamic mode decomposition. I.e. we use linear regression with LASSO regularization to learn an approximate linear model
with model structure

\begin{equation}
    \mathbf{\dot{x}} = A_{dmd}\mathbf{x} + B_{dmd}\mathbf{u}
\end{equation}

```python
#DMD parameters:
alpha_dmd = 1.4e-2                                                  # Regularization strength (LASSO) DMD
tune_mdl_dmd = False
```

```python
basis = lambda x: x
C_dmd = np.eye(n)

optimizer_dmd = linear_model.MultiTaskLasso(alpha=alpha_dmd, fit_intercept=False, selection='random')
cv_dmd = linear_model.MultiTaskLassoCV(fit_intercept=False, n_jobs=-1, cv=3, selection='random')
standardizer_dmd = preprocessing.StandardScaler(with_mean=False)

model_dmd = Edmd(n, m, basis, n, n_traj_dc, optimizer_dmd, cv=cv_dmd, standardizer=standardizer_dmd, C=C_dmd, first_obs_const=False)
xdmd, y_dmd = model_dmd.process(xs, us-hover_thrust, np.tile(t_eval,(n_traj_dc,1)), downsample_rate=sub_sample_rate)
model_dmd.fit(xdmd, y_dmd, cv=tune_mdl_dmd, override_kinematics=True)
sys_dmd = LinearLiftedDynamics(model_dmd.A, model_dmd.B, model_dmd.C, model_dmd.basis)
if tune_mdl_dmd:
    print('$\\alpha$ DMD: ',model_dmd.cv.alpha_)
```

### Learn a lifted linear model with extended dynamic mode decomposition (EDMD)

In addition, we compare our method with the current state of the art of Koopman based learning, the extended dynamic mode
decomposition. We use a dictionary of nonlinear functions $\boldsymbol{\phi(x)}$ to lift the state variables and learn a lifted state space model
of the dynamics. I.e. we first lift and then use linear regression with LASSO regularization to learn an approximate
lifted linear model with model structure

\begin{equation}
    \mathbf{\dot{z}} = A_{edmd}\mathbf{z} + B_{edmd}\mathbf{u}, \qquad \mathbf{z} = \boldsymbol{\phi(x)}
\end{equation}

```python
#EDMD parameters:
alpha_edmd = 1.1e-1                                                 # Regularization strength (LASSO) EDMD
tune_mdl_edmd = False
```

```python
basis = PolySineBasis(n, poly_deg=2, cross_terms=False)
basis.construct_basis()
poly_sine_features = preprocessing.FunctionTransformer(basis.basis)
poly_sine_features.fit(np.zeros((1,n)))
n_lift_edmd = poly_sine_features.transform((np.zeros((1,n)))).shape[1]
C_edmd = np.zeros((n,n_lift_edmd))
C_edmd[:,1:n+1] = np.eye(n)

optimizer_edmd = linear_model.MultiTaskLasso(alpha=alpha_edmd, fit_intercept=False, selection='random')
cv_edmd = linear_model.MultiTaskLassoCV(fit_intercept=False, n_jobs=-1, cv=3, selection='random')
standardizer_edmd = preprocessing.StandardScaler(with_mean=False)

model_edmd = Edmd(n, m, basis.basis, n_lift_edmd, n_traj_dc, optimizer_edmd, cv=cv_edmd, standardizer=standardizer_edmd, C=C_edmd)
X_edmd, y_edmd = model_edmd.process(xs, us-hover_thrust, np.tile(t_eval,(n_traj_dc,1)), downsample_rate=sub_sample_rate)
model_edmd.fit(X_edmd, y_edmd, cv=tune_mdl_edmd, override_kinematics=True)
model_edmd.reduce_mdl()
sys_edmd = LinearLiftedDynamics(model_edmd.A, model_edmd.B, model_edmd.C, model_edmd.basis_reduced)
if tune_mdl_edmd:
    print('$\\alpha$ EDMD: ',model_edmd.cv.alpha_)

```

### Learn a lifted bilinear model with bilinear extended mode decomposition (bEDMD)

Finally, we use the method developed in the paper to learn a lifted bilinear model of the dynamics, dubbed bilinear
extended mode decomposition (bEDMD). I.e. we first lift and then use linear regression with LASSO regularization to learn an approximate
lifted linear model with model structure

\begin{equation}
    \mathbf{\dot{z}}=F\mathbf{z}+\sum_{i=1}^m G_i\mathbf{z}\mathbf{u}_i, \qquad \mathbf{z} = \boldsymbol{\phi(x)}
\end{equation}

```python
#Bilinear EDMD parameters:
alpha_bedmd_init = 1.9e-2                                           # Regularization strength (LASSO) bEDMD
alpha_bedmd = 1.9e-2
tune_mdl_bedmd = False
```

```python
n_lift_bedmd = n_lift_edmd
C_bedmd = np.zeros((n,n_lift_bedmd))
C_bedmd[:,1:n+1] = np.eye(n)

basis_bedmd = lambda x: poly_sine_features.transform(x)
optimizer_bedmd = linear_model.MultiTaskLasso(alpha=alpha_bedmd, fit_intercept=False, selection='random')
cv_bedmd = linear_model.MultiTaskLassoCV(fit_intercept=False, n_jobs=-1, cv=3, selection='random')
standardizer_bedmd = preprocessing.StandardScaler(with_mean=False)

model_bedmd = BilinearEdmd(n, m, basis_bedmd, n_lift_bedmd, n_traj_dc, optimizer_bedmd, cv=cv_bedmd, standardizer=standardizer_bedmd, C=C_bedmd)
X_bedmd, y_bedmd = model_bedmd.process(xs, us-hover_thrust, np.tile(t_eval,(n_traj_dc,1)), downsample_rate=sub_sample_rate)
model_bedmd.fit(X_bedmd, y_bedmd, cv=tune_mdl_bedmd, override_kinematics=True)
sys_bedmd = BilinearLiftedDynamics(model_bedmd.n_lift, m, model_bedmd.A, model_bedmd.B, model_bedmd.C, model_bedmd.basis)
if tune_mdl_bedmd:
    print('$\\alpha$ bilinear EDMD: ', model_bedmd.cv.alpha_)

```

    /Users/carlaxelfolkestad/.conda/envs/keedmd/lib/python3.7/site-packages/sklearn/linear_model/_coordinate_descent.py:1790: ConvergenceWarning: Objective did not converge. You might want to increase the number of iterations. Duality gap: 370.434670698075, tolerance: 36.56953650773388
      check_random_state(self.random_state), random)

### Evaluate open loop prediction performance

We first evaluate the open loop prediction performance of the proposed method.
This is done by generating a new data set in the same way as the training set, predicting the evolution of the system
with the control sequence of each trajectory executed in the data set with each of the models, and finally comparing
the mean and standard deviation of the error between the true and predicted evolution over the trajectories. The
experimental results support what is to be expected from the theory as the error in the $y$ and $z$ terms are
significantly lower for the bEDMD method than both DMD and EDMD. The reason for this
improvement is that the bEDMD method can capture the nonlinearities present in the actuation matrix of the
$(y,z)$-dynamics.

```python
# Prediction performance evaluation parameters:
folder_plots = 'examples/figures/'                                  # Path to save plots
n_traj_ol = 100                                                      # Number of trajectories to execute, open loop
```

```python
xs_ol = np.empty((n_traj_ol, t_eval.shape[0], n))
xs_dmd_ol = np.empty((n_traj_ol, t_eval.shape[0]-1, n))
xs_edmd_ol = np.empty((n_traj_ol, t_eval.shape[0]-1, n))
xs_bedmd_ol = np.empty((n_traj_ol, t_eval.shape[0]-1, n))
us_test = np.empty((n_traj_ol, t_eval.shape[0]-1, m))

for ii in range(n_traj_ol):
    x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
    mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                QN_trajgen, set_pt_dc)
    mpc_trajgen.eval(x0, 0)
    xd = mpc_trajgen.parse_result().T

    while xd[0,0] is None:
        x0 = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        set_pt_dc = np.asarray([rand.uniform(l, u) for l, u in zip(-x0_max, x0_max)])
        mpc_trajgen = MPCController(nominal_sys, n_pred_dc, dt, umin, umax, xmin, xmax, QN_trajgen, R_trajgen,
                                    QN_trajgen, set_pt_dc)
        mpc_trajgen.eval(x0, 0)
        xd = mpc_trajgen.parse_result().T

    output = QuadrotorPdOutput(quadrotor, xd, t_eval, n, m)
    pd_controller = PDController(output, K_dc_p, K_dc_d)
    perturbed_pd_controller = PerturbedController(quadrotor, pd_controller, noise_var, const_offset=mass * gravity / 2)

    xs_ol[ii,:,:], us_test[ii,:,:] = quadrotor.simulate(x0, perturbed_pd_controller, t_eval)
    ol_controller_nom = OpenLoopController(sys_bedmd, us_test[ii,:,:]-hover_thrust, t_eval[:-1])

    xs_dmd_ol[ii,:,:], _ = sys_dmd.simulate(x0, ol_controller_nom, t_eval[:-1])

    z_0_edmd = sys_edmd.basis(np.atleast_2d(x0)).squeeze()
    zs_edmd_tmp, _ = sys_edmd.simulate(z_0_edmd, ol_controller_nom, t_eval[:-1])
    xs_edmd_ol[ii,:,:] = np.dot(model_edmd.C, zs_edmd_tmp.T).T

    z_0_bedmd = sys_bedmd.basis(np.atleast_2d(x0)).squeeze()
    zs_bedmd_tmp, _ = sys_bedmd.simulate(z_0_bedmd, ol_controller_nom, t_eval[:-1])
    xs_bedmd_ol[ii,:,:] = np.dot(model_bedmd.C, zs_bedmd_tmp.T).T

error_dmd = xs_ol[:,:-1,:] - xs_dmd_ol
error_dmd_mean = np.mean(error_dmd, axis=0).T
error_dmd_std = np.std(error_dmd, axis=0).T
mse_dmd = np.mean(np.mean(np.mean(np.square(error_dmd))))

error_edmd = xs_ol[:,:-1,:] - xs_edmd_ol
error_edmd_mean = np.mean(error_edmd, axis=0).T
error_edmd_std = np.std(error_edmd, axis=0).T
mse_edmd = np.mean(np.mean(np.mean(np.square(error_edmd))))

error_bedmd = xs_ol[:,:-1,:] - xs_bedmd_ol
error_bedmd_mean = np.mean(error_bedmd, axis=0).T
error_bedmd_std = np.std(error_bedmd, axis=0).T
mse_bedmd = np.mean(np.mean(np.mean(np.square(error_bedmd))))

print('\nOpen loop performance statistics:')
print('   MSE DMD:   ', "{:.3f}".format(mse_dmd),
      '\n   MSE EDMD:  ', "{:.3f}".format(mse_edmd),
      '\n   MSE bEDMD: ', "{:.3f}".format(mse_bedmd))
print('   Improvement DMD -> EDMD:   ', "{:.2f}".format((1 - mse_edmd / mse_dmd) * 100), ' %'
      '\n   Improvement DMD -> bEDMD:  ', "{:.2f}".format((1 - mse_bedmd / mse_dmd) * 100), ' %'
      '\n   Improvement EDMD -> bEDMD: ', "{:.2f}".format((1 - mse_bedmd / mse_edmd) * 100), ' %')
```

    Open loop performance statistics:
       MSE DMD:    18.290
       MSE EDMD:   9.704
       MSE bEDMD:  7.446
       Improvement DMD -> EDMD:    46.94  %
       Improvement DMD -> bEDMD:   59.29  %
       Improvement EDMD -> bEDMD:  23.27  %

```python
import matplotlib.pyplot as plt
import matplotlib

figwidth = 12
lw = 2
fs = 16
y_lim_gain = 1.2

#Plot open loop results:
ylabels = ['$e_{y}$', '$e_z$', '$e_{\\theta}$']
plt.figure(figsize=(figwidth,3))
for ii in range(3):
    plt.subplot(1,3,ii+1)
    plt.plot(t_eval[:-1], error_dmd_mean[ii,:], linewidth=lw, label='DMD')
    plt.fill_between(t_eval[:-1], error_dmd_mean[ii,:] - error_dmd_std[ii,:], error_dmd_mean[ii,:] + error_dmd_std[ii,:], alpha=0.2)
    plt.plot(t_eval[:-1], error_edmd_mean[ii, :], linewidth=lw, label='EDMD')
    plt.fill_between(t_eval[:-1], error_edmd_mean[ii, :] - error_edmd_std[ii, :],error_edmd_mean[ii, :] + error_edmd_std[ii, :], alpha=0.2)
    plt.plot(t_eval[:-1], error_bedmd_mean[ii, :], linewidth=lw, label='bEDMD')
    plt.fill_between(t_eval[:-1], error_bedmd_mean[ii, :] - error_bedmd_std[ii, :],error_bedmd_mean[ii, :] + error_bedmd_std[ii, :], alpha=0.2)
    ylim = max(max(np.abs(error_bedmd_mean[ii, :] - error_bedmd_std[ii, :])), max(np.abs(error_bedmd_mean[ii, :] + error_bedmd_std[ii, :])))
    plt.ylim([-ylim * y_lim_gain, ylim * y_lim_gain])
    plt.xlabel('$t$ (sec)', fontsize=fs)
    plt.ylabel(ylabels[ii], fontsize=fs)
    plt.grid()

plt.legend(loc='upper left', fontsize=fs-4)
suptitle = plt.suptitle('Open loop prediction error of DMD, EDMD and bilinear EDMD models', y=1.05, fontsize=18)
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
plt.tight_layout()
plt.savefig(folder_plots + 'planar_quad_prediction.pdf', format='pdf', dpi=2400, bbox_extra_artists=(suptitle,), bbox_inches="tight")
plt.show()

```

### Evaluate closed loop performance

We now study the closed loop performance of the control design. A trajectory is designed
to move the system from $(y_0,z_0) = (-1,0)$ to $(y_f,z_f) = (1,1)$. Then, the feedback linearizing controller is
designed to track the trajectory in the $z,\theta$ coordinates. These coordinates are chosen to avoid singularities
present in the $y,z$ coordinates. Comparing the closed loop performance of the feedback linearization based on the
bEDMD with LQR controllers designed using the DMD and EDMD models, we observe that the trajectory tracking error is
significantly reduced while expending only somewhat more control effort.

```python
#Closed loop performance evaluation parameters:
x0_cl = np.array([-1., 0., 0., 0., 0., 0.])                         # Initial value, closed loop trajectory
set_pt_cl = np.array([1., 1., 0., 0., 0., 0.])                      # Desired final value, closed loop trajectory
t_eval_cl = dt * np.arange(201)                                     # Simulation time points, closed loop
Q_trajgen_cl = sc.sparse.diags([0,0,0,0,0,0])                       # State penalty matrix, trajectory generation
QN_trajgen_cl = sc.sparse.diags([3e2,3e2,3e2,1e2,1e2,1e2])          # Final state penalty matrix, trajectory generation
R_trajgen_cl = sc.sparse.eye(m)                                     # Actuation penalty matrix, trajectory generation
mpc_trajgen_cl = MPCController(nominal_sys,t_eval_cl.size,dt,umin,umax,xmin,xmax,QN_trajgen_cl,R_trajgen_cl,QN_trajgen_cl,set_pt_cl)
q_cl, r_cl = 2e1, 1                                                  # State and actuation penalty values, closed loop
output_inds = np.array([1, 2])                                      # Output states, feedback linearizing controller

# Generate trajectory:
mpc_trajgen_cl.eval(x0_cl, 0)
xr_cl = mpc_trajgen_cl.parse_result()[:,:-1]
ur_cl = mpc_trajgen_cl.get_control_prediction()
xr_cl_dot = nominal_sys.eval_dot(xr_cl,ur_cl,0.)

# Define outputs:
y_d = xr_cl[output_inds,:]
y_d_dot = xr_cl[output_inds+int(n/2),:]
y_d_ddot = xr_cl_dot[output_inds+int(n/2),:]
```

##### Design LQR controller based on DMD model

```python
Q_dmd = q_cl*np.identity(n)
R_dmd = r_cl*np.identity(m)
P_dmd = sc.linalg.solve_continuous_are(model_dmd.A, model_dmd.B, Q_dmd, R_dmd)
K_dmd = np.linalg.inv(R_dmd)@model_dmd.B.T@P_dmd
K_dmd_p, K_dmd_d = K_dmd[:,:int(n/2)], K_dmd[:,int(n/2):]
output_dmd = QuadrotorPdOutput(sys_dmd, xr_cl.T, t_eval_cl, n, m)
controller_dmd = PDController(output_dmd, K_dmd_p, K_dmd_d)
controller_dmd = PerturbedController(sys_dmd,controller_dmd,0.,const_offset=hover_thrust, umin=umin, umax=umax)
```

##### Design LQR controller based on EDMD model

```python
z_d_edmd = np.array([sys_edmd.basis(x.reshape(1,-1)).squeeze() for x in xr_cl.T]).T
z_d_dot_edmd = differentiate_vec(z_d_edmd.T, t_eval_cl).T
z_d_ddot_edmd = differentiate_vec(z_d_dot_edmd.T, t_eval_cl).T
output_edmd = QuadrotorTrajectoryOutput(sys_edmd, y_d, y_d_dot, y_d_ddot, dt, z_d_edmd, z_d_dot_edmd, z_d_ddot_edmd, model_edmd.C[output_inds,:])

Q_edmd = q_cl*np.identity(sys_edmd.n)
R_edmd = r_cl*np.identity(m)
P_edmd = sc.linalg.solve_continuous_are(sys_edmd.A, sys_edmd.B, Q_edmd, R_edmd)
K_edmd = np.linalg.inv(R_edmd)@model_edmd.B.T@P_edmd
controller_edmd = LinearLiftedController(output_edmd, K_edmd)
controller_edmd = PerturbedController(quadrotor, controller_edmd,0.,const_offset=hover_thrust, umin=umin, umax=umax)

```

##### Design feedback linearizing controller based on bEDMD model

```python
k = m
n_lift_bedmd = sys_bedmd.n
Q_bedmd = q_cl*np.eye(int(2*n_lift_bedmd))
R_bedmd = r_cl*np.eye(n_lift_bedmd)
C_h = model_bedmd.C[output_inds,:]

z_d_bedmd = np.array([sys_bedmd.basis(x.reshape(1,-1)).squeeze() for x in xr_cl.T]).T
z_d_dot_bedmd = differentiate_vec(z_d_bedmd.T, t_eval_cl).T
z_d_ddot_bedmd = differentiate_vec(z_d_dot_bedmd.T, t_eval_cl).T
output_bedmd = QuadrotorTrajectoryOutput(sys_bedmd, y_d, y_d_dot, y_d_ddot, dt, z_d_bedmd, z_d_dot_bedmd, z_d_ddot_bedmd, C_h)

f_eta = np.concatenate((np.zeros((n_lift_bedmd,n_lift_bedmd)), np.eye(n_lift_bedmd)), axis=1)
f_eta_dot = np.concatenate((sys_bedmd.F@sys_bedmd.F, np.zeros((n_lift_bedmd,n_lift_bedmd))), axis=1)
F_lin = np.concatenate((f_eta, f_eta_dot), axis=0)
G_lin = np.concatenate((np.zeros((n_lift_bedmd,n_lift_bedmd)), np.eye(n_lift_bedmd)), axis=0)

P_bedmd = sc.linalg.solve_continuous_are(F_lin, G_lin, Q_bedmd, R_bedmd)
K_bedmd = np.linalg.inv(R_bedmd)@G_lin.T@P_bedmd
controller_bedmd = BilinearFBLinController(sys_bedmd, output_bedmd, K_bedmd)
controller_bedmd = PerturbedController(sys_bedmd, controller_bedmd,0.,const_offset=hover_thrust, umin=umin, umax=umax)

```

##### Compare closed loop performance

```python
# Simulate the system under closed loop control:
xs_cl_dmd, us_cl_dmd = quadrotor.simulate(x0_cl, controller_dmd, t_eval_cl)
xs_cl_edmd, us_cl_edmd = quadrotor.simulate(x0_cl, controller_edmd, t_eval_cl)
xs_cl_bedmd, us_cl_bedmd = quadrotor.simulate(x0_cl, controller_bedmd, t_eval_cl)

mse_cl_dmd = np.linalg.norm(xs_cl_dmd[1:,output_inds]-xr_cl[output_inds,1:].T, ord='fro')**2
mse_cl_edmd = np.linalg.norm(xs_cl_edmd[1:,output_inds]-xr_cl[output_inds,1:].T, ord='fro')**2
mse_cl_bedmd = np.linalg.norm(xs_cl_bedmd[1:,output_inds]-xr_cl[output_inds,1:].T, ord='fro')**2
ctrl_cost_dmd = np.linalg.norm(us_cl_dmd, ord='fro')**2
ctrl_cost_edmd = np.linalg.norm(us_cl_edmd, ord='fro')**2
ctrl_cost_bedmd = np.linalg.norm(us_cl_bedmd, ord='fro')**2

print('\nClosed loop performance statistics:')
print(' -Tracking error:')
print('   Tracking MSE DMD:   ', "{:.3f}".format(mse_cl_dmd),
      '\n   Tracking MSE EDMD:  ', "{:.3f}".format(mse_cl_edmd),
      '\n   Tracking MSE bEDMD: ', "{:.3f}".format(mse_cl_bedmd))
print('   Improvement DMD -> EDMD:   ', "{:.2f}".format(100*(1-(mse_cl_edmd)/(mse_cl_dmd))), ' %'
      '\n   Improvement DMD -> bEDMD:  ', "{:.2f}".format(100*(1-(mse_cl_bedmd)/(mse_cl_dmd))), ' %'
      '\n   Improvement EDMD -> bEDMD: ', "{:.2f}".format(100*(1-(mse_cl_bedmd)/(mse_cl_edmd))), ' %')
print(' -Control effort:')
print('   Control effort DMD:   ', "{:.3f}".format(ctrl_cost_dmd),
      '\n   Control effort EDMD:  ', "{:.3f}".format(ctrl_cost_edmd),
      '\n   Control effort bEDMD: ', "{:.3f}".format(ctrl_cost_bedmd))
print('   Improvement DMD -> EDMD:   ', "{:.2f}".format(100*(1-(ctrl_cost_edmd)/(ctrl_cost_dmd))), ' %'
      '\n   Improvement DMD -> bEDMD:  ', "{:.2f}".format(100*(1-(ctrl_cost_bedmd)/(ctrl_cost_dmd))), ' %'
      '\n   Improvement EDMD -> bEDMD: ', "{:.2f}".format(100*(1-(ctrl_cost_bedmd)/(ctrl_cost_edmd))), ' %')

```

```python
x_index = output_inds[0]
y_index = output_inds[1]
plt.figure(figsize=(figwidth, 4))

plt.subplot(2, 2, 1)
plt.plot(t_eval_cl, xr_cl[x_index,:], '--r', linewidth=2, label='Reference')
plt.plot(t_eval_cl,xs_cl_dmd[:, x_index], linewidth=lw, label='DMD')
plt.plot(t_eval_cl,xs_cl_edmd[:, x_index], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl,xs_cl_bedmd[:, x_index], linewidth=lw, label='bEDMD')
plt.ylabel('$z$', fontsize=fs)
plt.title('Output states', fontsize=fs)
plt.grid()

plt.subplot(2, 2, 3)
plt.plot(t_eval_cl, xr_cl[y_index, :], '--r', linewidth=2, label='Reference')
plt.plot(t_eval_cl, xs_cl_dmd[:, y_index], linewidth=lw, label='DMD')
plt.plot(t_eval_cl, xs_cl_edmd[:, y_index], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl, xs_cl_bedmd[:, y_index], linewidth=lw, label='bEDMD')
plt.ylabel('$\\theta$', fontsize=fs)
plt.xlabel('Time (sec)')
plt.grid()

plt.subplot(2, 2, 2)
plt.plot(t_eval_cl[:-1], us_cl_dmd[:, 0], linewidth=lw, label='DMD')
plt.plot(t_eval_cl[:-1], us_cl_edmd[:, 0], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl[:-1], us_cl_bedmd[:, 0], linewidth=lw, label='bEDMD')
plt.ylabel('$u_1$', fontsize=fs)
plt.title('Control action', fontsize=fs)
plt.grid()

plt.subplot(2, 2, 4)
plt.plot(t_eval_cl[:-1], us_cl_dmd[:, 1], linewidth=lw, label='DMD')
plt.plot(t_eval_cl[:-1], us_cl_edmd[:, 1], linewidth=lw, label='EDMD')
plt.plot(t_eval_cl[:-1], us_cl_bedmd[:, 1], linewidth=lw, label='bEDMD')
plt.xlabel('Time (sec)', fontsize=fs)
plt.ylabel('$u_2$', fontsize=fs)
plt.grid()

suptitle = plt.suptitle('Trajectory tracking based on DMD, EDMD and bilinear EDMD models', y=1.05,fontsize=18)
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
plt.tight_layout()
plt.savefig(folder_plots + 'planar_quad_closedloop.pdf', format='pdf', dpi=2400, bbox_extra_artists=(suptitle,),
            bbox_inches="tight")
plt.show()
```


# SARSA Learning

Originally from https://skettee.github.io/post/sarsa_learning/ (in Korean)

## Load Libraries and Extensions

```python
%reload_ext autoreload
%autoreload 2
%matplotlib inline
```

```python
from IPython.display import display, clear_output, Pretty
import numpy as np
from time import sleep
from tqdm import tqdm_notebook as tqdm

import gym
```

## Frozen Lake Environment

```python
ENV_NAME = 'FrozenLake8x8-v0'
N_STEP = 100
```

```python
env = gym.make(ENV_NAME, is_slippery=False)
state = env.reset()

world = env.render(mode='ansi')
display(Pretty(world))
sleep(0.5)
```

    SFFFFFFF
    FFFFFFFF
    FFFHFFFF
    FFFFFHFF
    FFFHFFFF
    FHHFFFHF
    FHFFHFHF
    FFFHFFFG

```python
for step in range(N_STEP):
    action =env.action_space.sample()
    next_state, reward, done, info = env.step(action)
    state = next_state

    # updated world display
    world = env.render(mode='ansi')
    clear_output(wait=True)
    display(Pretty(world))
    sleep(0.5)

    if done: # an episode finished
        print("Episode finished after {} timesteps".format(step+1))
        break
```

      (Down)
    SFFFFFFF
    FFFFFFFF
    FFFHFFFF
    FFFFFHFF
    FFFHFFFF
    FHHFFFHF
    FHFFHFHF
    FFFHFFFG

    Episode finished after 48 timesteps

```python
env.observation_space
```

    Discrete(64)

There are 64 states as follows:

$S = \{0, 1, \cdots , 63\}$

$\begin{vmatrix}
0 & 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
\end{vmatrix}$

There are 10 terminal states as follows:

$S_{\text{terminal}} = \{19, 29, 35, 41, 42, 49, 52, 54, 59, 63\}$

## SARSA Learning

$\begin{align}
q_{\pi}(s,a) & = \mathbb E_{\pi} [G_t | S_t = s, A_t = a] \\
&= \mathbb E_{\pi} [R_{t+1} + \gamma q_{\pi}(S_{t+1}, A_{t+1}) | S_t = s, A_t = a]
\end{align}$

$q_*(s,a) = \max_{\pi} q_{\pi}(s,a)$

$\pi_*(s,a) = \begin{cases}
1 & \text{if } a= \text{argmax}_{a \in A} q_\star(s,a) \\
0 & \text{otherwise}
\end{cases}$

$Q(S_t, A_t) \leftarrow Q(S_t, A_t) + \alpha \left( R_{t+1} + \gamma Q(S_{t+1}, A_{t+1}) - Q(S_t, A_t) \right)$

$\pi \leftarrow \epsilon \text{-greedy(Q)}$

```python
n_state = env.observation_space.n
n_action = env.action_space.n
n_episode = 1000
GAMMA = .9
EPSILON = .3
ALPHA = .1
```

```python
terminal_states = [19, 29, 35, 41, 42, 49, 52, 54, 59, 63]

Q_table = np.random.uniform(low=0.0, high=0.00000001, size=(n_state, n_action))
for s in terminal_states:
    Q_table[s] = 0

for episode in tqdm(range(n_episode)):
    state = env.reset()
    done = False

    if np.random.uniform() < EPSILON:
        action = env.action_space.sample()
    else:
        action = np.argmax(Q_table[state])

    while not done:
        next_state, reward, done, info = env.step(action)
        if np.random.uniform() < EPSILON:
            next_action = env.action_space.sample()
        else:
            next_action = np.argmax(Q_table[next_state])

        target = reward + GAMMA * Q_table[next_state, next_action]
        delta = target - Q_table[state][action]
        Q_table[state][action] += ALPHA * delta
        state, action = next_state, next_action
```

    HBox(children=(IntProgress(value=0, max=1000), HTML(value='')))

## Solution

```python
state = env.reset()
done = False

world = env.render(mode='ansi')
display(Pretty(world))
sleep(.5)

while not done:
    action = np.argmax(Q_table[state])
    state, reward, done, info = env.step(action)

    world = env.render(mode='ansi')
    clear_output(wait=True)
    display(Pretty(world))
    sleep(.5)

    if done and state == 63:
        print('\nSuccess!')
```

      (Down)
    SFFFFFFF
    FFFFFFFF
    FFFHFFFF
    FFFFFHFF
    FFFHFFFF
    FHHFFFHF
    FHFFHFHF
    FFFHFFFG

    Success!

```python

```