Skip to content

Instantly share code, notes, and snippets.

@baggepinnen
Created June 7, 2022 06:20
Show Gist options
  • Save baggepinnen/706163f75c5698c1490d679d354ec9a6 to your computer and use it in GitHub Desktop.
Save baggepinnen/706163f75c5698c1490d679d354ec9a6 to your computer and use it in GitHub Desktop.
Feedback linearization using Symbolics
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)
@ValentinKaisermayer
Copy link

ValentinKaisermayer commented Jun 7, 2022

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment