Feedback linearization using Symbolics

feedbacklin.jl

using Symbolics

"""
    lie(f, h, x)

The Lie derivative of `h` w.r.t. `x` in the direction of `f`.
"""
lie(f, h, x) = Symbolics.gradient(h, x)'f

"""
    Lfh, LgLfh, T, ν = feedbacklin(f, g, h, x, u)

Feedback linearize a SISO input-output dynamical system on input-affine form
```math
ẋ = f(x) + g(x)u \\
y = h(x)
```

The feedback-linearizing control law is given by
```math
u = \\dfrac{1}{L_g L_f^{ν-1} h}  (r - L_f^ν h)
```
which is realized by
```
u = 1/LgLfh * (r - Lfh)
```
where `r` is the new input and `Lfh` and `LgLfh` are symbolic expressions in `x` representing the Lie-derivatives. The linearized system from `r` to `y` will behave like a chain of `ν` integrators. 

`T` contains all the Lie derivatives ``L_fh`` from order 0 (i.e., only ``h``) to order ``ν-1``.

# Arguments:
- `f`: A vector of symbolic expressions representing `f(x)` 
- `g`: A vector of symbolic expressions representing `g(x)` (a static numeric array is okay if `g` is not a function of `x`)
- `h`: A symbolic expression representing `h(x)`
- `x`: A vector of the symbolic `x` variables
- `u`: The symbolic scalar variable `u`
"""
function feedbacklin(f, g, h, x, u)
    n = length(x)
    T = [h]
    Lfh = h
    local LgLfh
    for i = 1:n
        LgLfh = lie(g, Lfh, x)
        Lfh   = lie(f, Lfh, x)
        push!(T, Lfh)
        if !iszero(LgLfh)
            # the relative degree is the smallest number ν such that LgLfᵛ⁻¹h ≢ 0
            # i = ν-1
            return Lfh, LgLfh, T, i+1
        end
    end
    error("The control signal did not appear after n = length(x) differentiations, this indicates that the system is uncontrollable.")
end


## Tests

@variables x1 x2 u
x = [x1; x2]
f = [
    x2^3
    0
]
g = [0; 1]
h = x1 + x2

Lh, Lg, T, n = feedbacklin(f, g, h, x, u)
@test isequal(Lh, x2^3)
@test isequal(Lg, 1)
@test n == 2

## The example below does not work, not sure why

# @variables x[1:3] u
# x = collect(x)
# f = [
#     x[2]
#     sin(x[3])
#     0
# ]
# g = [0; 0; 1]
# h = x[1]
# feedbacklin(f, g, h, x, u)

##

@variables x[1:3] u
@variables a
x = collect(x)
f = [
    sin(x[2]) + (x[2]+1)*x[3]
    x[1]^5 + x[3]
    x[1]^2
]
g = [0; 0; 1]
h = x[1]

Lh, Lg, T, n = feedbacklin(f, g, h, x, u)
@test isequal(Lh, (x[1]^2)*(1 + x[2]) + (x[1]^5 + x[3])*(cos(x[2]) + x[3]))
@test isequal(Lg, x[2]+1)
@test n == 3

##

@variables x[1:3] u
@variables a
x = collect(x)
f = [
    -x[1]
    2x[1]*x[2] + sin(x[2])
    2x[2]
]
g = [
    exp(2x[2])
    1/2
    0
]
h = x[3]

Lh, Lg, T, n = feedbacklin(f, g, h, x, u)

Here is my implementation (certainly not efficient):

function myLie(f, h, x, n)
    if n<=0 
        return h
    else
        return Symbolics.gradient(myLie(f, h, x, n-1), x)'f
    end
end

function relDegree(f, g, h, x)
    for d in 1:length(f)
        if !all(iszero.(myLie(g, myLie(f, h, x, d-1), x, 1)))
            return d
        end
    end
end

rel_degree = relDegree(f, g, h, x)

if rel_degree == length(f)
    @info("System has relative degree of $rel_degree; can be input-state linearized")
    T = [myLie(f, h, x, d) + myLie(g, myLie(f, h, x, d-1), x, 1) for d in 0:rel_degree-1]
else
    @info("System has relative degree of $rel_degree; can only be input-output linearized i.e. transformed system will be non-linear.")
    T = vcat(
        [myLie(f, h, x, d) + myLie(g, myLie(f, h, x, d-1), x, 1) for d in 0:rel_degree-1],
        [Symbolics.variable(:t, length(f)-rel_degree)], # non-unique choice!
    )
end

# inverse feedback transformation
v = myLie(f, h, x, rel_degree) + myLie(g, myLie(f, h, x, rel_degree-1), x, 1) * u 

@info """
Inverse state transformation: $(T)
Inverse feedback transformation: $(v)
"""