l. vandenberghe ece133b (spring 2020) 11. constrained...
TRANSCRIPT
![Page 1: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/1.jpg)
L. Vandenberghe ECE133B (Spring 2020)
11. Constrained nonlinear least squares
• Lagrange multipliers
• constrained nonlinear least squares
• penalty method
• augmented Lagrangian method
• nonlinear control example
11.1
![Page 2: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/2.jpg)
Constrained nonlinear least squares
Nonlinear least squares
minimize f1(x)2 + · · · + fm(x)2 = ‖ f (x)‖2
• variable is n-vector x
• fi(x) is ith (scalar) residual
• f : Rn → Rm is the vector function f (x) = ( f1(x), . . . , fm(x))
Algorithms (see 133A)
• Newton’s method
• Gauss–Newton method
• Levenberg–Marquardt method
This lecture: add p equality constraints
g1(x) = 0, g2(x) = 0, . . . , gp(x) = 0
Constrained nonlinear least squares 11.2
![Page 3: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/3.jpg)
Outline
• Lagrange multipliers
• constrained nonlinear least squares
• penalty method
• augmented Lagrangian method
• nonlinear control example
![Page 4: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/4.jpg)
Notation
we use the same derivative notation as in 133A
• gradient and Hessian of a scalar function h : Rn → R are denoted by
∇h(x) =
∂h∂x1
(x)...
∂h∂xn
(x)
, ∇2h(x) =
∂2h
∂x21(x) · · ·
∂2h∂x1∂xn
(x)
... ...
∂2h∂xn∂x1
(x) · · ·∂2h
∂x2n(x)
• Jacobian of vector function f : Rn → Rm is denoted by
D f (x) =
∂ f1∂x1
(x) · · ·∂ f1∂xn
(x)... ...
∂ fm∂x1
(x) · · ·∂ fm∂xn
(x)
=
∇ f1(x)T...
∇ fm(x)T
Constrained nonlinear least squares 11.3
![Page 5: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/5.jpg)
Minimization with equality constraints
minimize h(x)subject to g1(x) = 0
. . .gp(x) = 0
h, g1, . . . , gp are functions from Rn to R
• x is feasible if it satisfies the constraints:
g(x) =g1(x)...
gp(x)
= 0
• feasible x is optimal (or a minimum) if h(x) ≤ h(x) for all feasible x
• feasible x is locally optimal (local minimum) if there exists an R > 0 such that
h(x) ≤ h(x) for all feasible x with ‖x − x‖ ≤ R
Constrained nonlinear least squares 11.4
![Page 6: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/6.jpg)
Lagrange multipliers
Lagrangian: the Lagrangian is the function
L(x, z) = h(x) + zTg(x)
= h(x) + z1g1(x) + · · · + zpgp(x)
the p-vector z = (z1, . . . , zp) is vector of Lagrange multipliers z1, . . . , zp
Gradient of Lagrangian
∇L(x, z) =[∇xL(x, z)∇zL(x, z)
]where
∇xL(x, z) = ∇h(x) + z1∇g1(x) + · · · + zp∇gp(x)
= ∇h(x) + Dg(x)T z
∇zL(x, z) = g(x)
Constrained nonlinear least squares 11.5
![Page 7: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/7.jpg)
First-order optimality conditions
minimize h(x)subject to g(x) = 0
h is a function from Rn to R, g is a function from Rn to Rp
First-order necessary optimality conditions
if x is locally optimal and rank(Dg(x)) = p, then there exist multipliers z with
∇Lx(x, z) = ∇h(x) + Dg(x)T z = 0
• together with g(x) = 0, this forms a set of n+ p equations in n+ p variables x, z
• gradient ∇h(x) is linear combination of gradients ∇g1(x), . . . , ∇gp(x)
Regular feasible point
• a feasible x with if rank(Dg(x)) = p is called a regular feasible point
• a regular feasible point, gradients ∇g1(x), . . . , ∇gp(x) are linearly independent
Constrained nonlinear least squares 11.6
![Page 8: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/8.jpg)
Example
suppose A is a symmetric n × n matrix
minimize xT Axsubject to xT x = 1
• Lagrangian isL(x, z) = xT Ax + z(xT x − 1)
• first-order necessary optimality condition:
xT x = 1, ∇xL(x, z) = 0 ⇐⇒ xT x = 1, Ax = −z x
• hence optimal x must be an eigenvector
Constrained nonlinear least squares 11.7
![Page 9: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/9.jpg)
Example
minimize x2subject to x2
1 + x22 = 1
(x1 − 2)2 + x22 = 1
x1
x2
x = (1,0)
• x = (1,0) is the only feasible point, hence optimal
• Lagrangian is L(x, z) = x2 + z1(x21 + x2
2 − 1) + z2((x1 − 2)2 + x22 − 1)
• 1st order optimality condition at x = (1,0):
0 = ∇xL(x, z) =[
01
]+ 2z1
[x1x2
]+ 2z2
[x1 − 2
x2
]=
[01
]+ 2z1
[10
]+ 2z2
[−10
]• this does not hold for any z1, z2
• x is not a regular point: gradients (2,0) and (−2,0) are linearly dependent
Constrained nonlinear least squares 11.8
![Page 10: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/10.jpg)
Outline
• Lagrange multipliers
• constrained nonlinear least squares
• penalty method
• augmented Lagrangian method
• nonlinear control example
![Page 11: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/11.jpg)
Constrained nonlinear least squares
minimize f1(x)2 + · · · + fm(x)2
subject to g1(x) = 0, . . . , gp(x) = 0
• variable is n-vector x
• fi(x) is ith (scalar) residual
• gi(x) = 0 is ith (scalar) equality constraint
Vector notationminimize ‖ f (x)‖2
subject to g(x) = 0
• f : Rn → Rm is vector function f (x) = ( f1(x), . . . , fm(x))
• g : Rn → Rp is vector function g(x) = (g1(x), . . . ,gp(x))
Constrained nonlinear least squares 11.9
![Page 12: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/12.jpg)
First-order necessary optimality condition
Lagrangian
L(x, z) = f1(x)2 + · · · + fm(x)2 + z1g1(x) + · · · + zpgp(x)
= ‖ f (x)‖2 + zTg(x)
Gradients of Lagrangian: ∇zL(x, z) = g(x) and
∇xL(x, z) = 2D f (x)T f (x) + Dg(x)T z
= 2[∇ f1(x) · · · ∇ fm(x)
] f1(x)...
fm(x)
+[∇g1(x) · · · ∇gp(x)
] z1...
zp
Optimality condition: if x is locally optimal, then there exists z such that
2D f (x)T f (x) + Dg(x)T z = 0, g(x) = 0
(provided the rows of Dg(x) are linearly independent)Constrained nonlinear least squares 11.10
![Page 13: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/13.jpg)
Constrained (linear) least squares
minimize ‖Ax − b‖2
subject to Cx = d
• a special case of the nonlinear problem with
f (x) = Ax − b, g(x) = Cx − d
• apply general optimality condition:
2D f (x)T f (x) + Dg(x)T z = 2AT(Ax − b) + CT z = 0, g(x) = Cx − d = 0
• these are the Karush–Kuhn–Tucker (KKT) equations[2AT A CT
C 0
] [xz
]=
[2AT b
d
]
Constrained nonlinear least squares 11.11
![Page 14: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/14.jpg)
Outline
• Lagrange multipliers
• constrained nonlinear least squares
• penalty method
• augmented Lagrangian method
• nonlinear control example
![Page 15: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/15.jpg)
Penalty method
solve a sequence of (unconstrained) nonlinear least squares problems
minimize ‖ f (x)‖2 + µ‖g(x)‖2 =
[ f (x)õg(x)
] 2
• µ is a positive penalty parameter
• instead of insisting on g(x) = 0 we assign a penalty to deviations from zero
• for increasing sequence µ(1), µ(2), . . . , we compute x(k+1) by minimizing
‖ f (x)‖2 + µ(k)‖g(x)‖2
• x(k+1) is computed by Levenberg–Marquardt algorithm started at x(k)
Constrained nonlinear least squares 11.12
![Page 16: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/16.jpg)
Termination
optimality condition for constrained nonlinear least squares problem:
2D f (x)T f (x) + Dg(x)T z = 0, g(x) = 0 (1)
• x(k) in penalty method satisfies normal equations for linear least squares:
2D f (x(k))T f (x(k)) + 2µ(k−1)Dg(x(k))Tg(x(k)) = 0
• if we define z(k) = 2µ(k−1)g(x(k)), this can be written as
2D f (x(k))T f (x(k)) + Dg(x(k))T z(k) = 0
• we see that x(k), z(k) satisfy first equation in optimality condition (1)
• feasibility g(x(k)) = 0 is only satisfied approximately for µ(k−1) large enough
• penalty method is terminated when ‖g(x(k))‖ becomes sufficiently small
Constrained nonlinear least squares 11.13
![Page 17: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/17.jpg)
Example
f (x1, x2) =
[x1 + exp(−x2)x2
1 + 2x2 + 1
], g(x1, x2) = x1 + x3
1 + x2 + x22
−1 0 1−1
0
1
g(x) = 0
g(x) = −1
g(x) = 1
x
x1
x 2
: contour lines of ‖ f (x)‖2
: minimizer of ‖ f (x)‖2
: contour lines of g(x)
: solution x
Constrained nonlinear least squares 11.14
![Page 18: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/18.jpg)
First six iterations
−0.5
0
0.5
x(2)
µ(1) = 1
x(3)
µ(2) = 2
x(4)
µ(3) = 4
−0.5 0 0.5−0.5
0
0.5
x(5)
µ(4) = 8
−0.5 0 0.5
x(6)
µ(5) = 16
−0.5 0 0.5
x(7)
µ(6) = 32
: contour lines of ‖ f (x)‖2 + µ(k)‖g(x)‖2
Constrained nonlinear least squares 11.15
![Page 19: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/19.jpg)
Convergence
0 20 40 60 80 100 120 14010−6
10−4
10−2
100
102
Cumulative L–M iterations
Resi
dual
FeasibilityOptimality
0 20 40 60 80 100 120 140100
101
102
103
104
Cumulative L–M iterations
Pena
ltypa
ram
eterµ
figure on the left shows the two residuals in optimality condition:
• blue curve is norm of g(x(k))
• red curve is norm of 2D f (x(k))T f (x(k)) + Dg(x(k))T z(k)
Constrained nonlinear least squares 11.16
![Page 20: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/20.jpg)
Drawback of penalty method
• µ(k) increases rapidly and must become large to drive g(x) to (near) zero
• for large µ(k), nonlinear least squares subproblem becomes harder
• for large µ(k), Levenberg–Marquardt method can take many iterations, or fail
Constrained nonlinear least squares 11.17
![Page 21: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/21.jpg)
Outline
• Lagrange multipliers
• constrained nonlinear least squares
• penalty method
• augmented Lagrangian method
• nonlinear control example
![Page 22: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/22.jpg)
Augmented Lagrangian
the augmented Lagrangian for the constrained NLLS problem is
Lµ(x, z) = L(x, z) + µ‖g(x)‖2
= ‖ f (x)‖2 + g(x)T z + µ‖g(x)‖2
• this is the Lagrangian L(x, z) augmented with a quadratic penalty
• µ is a positive penalty parameter
• augmented Lagrangian is the Lagrangian of the equivalent problem
minimize ‖ f (x)‖2 + µ‖g(x)‖2
subject to g(x) = 0
Constrained nonlinear least squares 11.18
![Page 23: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/23.jpg)
Minimizing augmented Lagrangian
• equivalent expressions for augmented Lagrangian
Lµ(x, z) = ‖ f (x)‖2 + g(x)T z + µ ‖g(x)‖2
= ‖ f (x)‖2 + µ ‖g(x) +1
2µz‖2 −
12µ
‖z‖2
=
[ f (x)õg(x) + z/(2õ)
] 2−
12µ
‖z‖2
• can be minimized over x (for fixed µ, z) by Levenberg–Marquardt method:
minimize [ f (x)
õg(x) + z/(2õ)
] 2
Constrained nonlinear least squares 11.19
![Page 24: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/24.jpg)
Lagrange multiplier update
optimality conditions for constrained nonlinear least squares problem:
2D f (x)T f (x) + Dg(x)T z = 0, g(x) = 0
• minimizer x of augmented Lagrangian Lµ(x, z) satisfies
2D f (x)T f (x) + Dg(x)T(2µg(x) + z) = 0
• first equation in optimality condition is satisfied if we define
z = z + 2µg(x)
• this shows that if g(x) = 0, then x is optimal
• if g(x) is not small, suggests z is a good update for z
Constrained nonlinear least squares 11.20
![Page 25: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/25.jpg)
Augmented Lagrangian algorithm
1. set x(k+1) to be the (approximate) minimizer of
‖ f (x)‖2 + µ(k)‖g(x) + z(k)/(2µ(k))‖2
x(k+1) is computed using Levenberg–Marquardt algorithm, starting from x(k)
2. multiplier update:z(k+1) = z(k) + 2µ(k)g(x(k+1))
3. penalty parameter update:
µ(k+1) =
{µ(k) if ‖g(x(k+1))‖ < 0.25‖g(x(k))‖2µ(k) otherwise
• iteration starts at z(1) = 0, µ(1) = 1, some initial x(1)
• µ is increased only when needed, more slowly than in penalty method
• continues until g(x(k)) is sufficiently small (or iteration limit is reached)
Constrained nonlinear least squares 11.21
![Page 26: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/26.jpg)
Example of slide 11.14
−0.5
0
0.5
x(2)
µ(1) = 1, z(1) = 0
x(3)
µ(2) = 2, z(2) = −0.893
x(4)
µ(3) = 4, z(3) = −1.569
−0.5 0 0.5−0.5
0
0.5
x(5)
µ(4) = 4, z(4) = −1.898
−0.5 0 0.5
x(6)
µ(5) = 4, z(5) = −1.976
−0.5 0 0.5
x(7)
µ(6) = 4, z(6) = −1.994
: contour lines of augmented Lagrangian Lµ(k)(x, z(k))
Constrained nonlinear least squares 11.22
![Page 27: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/27.jpg)
Convergence
0 20 40 60 80 100 120
10−4
10−2
100
102
Cumulative L–M iterations
Resi
dual
FeasibilityOptimality
0 20 40 60 80 100 120
1
2
4
Cumulative L–M iterations
Pena
ltypa
ram
eterµ
figure on the left shows residuals in optimality condition:
• blue curve is norm of g(x(k))
• red curve is norm of 2D f (x(k))T f (x(k)) + Dg(x(k))T z(k)
Constrained nonlinear least squares 11.23
![Page 28: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/28.jpg)
Outline
• Lagrange multipliers
• constrained nonlinear least squares
• penalty method
• augmented Lagrangian method
• nonlinear control example
Constrained nonlinear least squares 11.23
![Page 29: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/29.jpg)
Simple model of a car
L
θ
φ
(p1, p2)
dp1dt
= s(t) cos θ(t)
dp2dt
= s(t) sin θ(t)
dθdt=
s(t)L
tan φ(t)
• s(t) is speed of vehicle
• φ(t) is steering angle
• p(t) is position
• θ(t) is orientation
Constrained nonlinear least squares 11.24
![Page 30: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/30.jpg)
Discretized model
• discretized model (for small time interval h):
p1(t + h) ≈ p1(t) + hs(t) cos(θ(t))
p2(t + h) ≈ p2(t) + hs(t) sin(θ(t))
θ(t + h) ≈ θ(t) + hs(t)L
tan(φ(t))
• define input vector uk = (s(kh), φ(kh))
• define state vector xk = (p1(kh), p2(kh), θ(kh))
• discretized model is xk+1 = f (xk,uk) with
f (xk,uk) =
(xk)1 + h(uk)1 cos((xk)3)(xk)2 + h(uk)1 sin((xk)3)
(xk)3 + h(uk)1 tan((uk)2)/L
Constrained nonlinear least squares 11.25
![Page 31: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/31.jpg)
Control problem
• move car from given initial to desired final position and orientation
• using a small and slowly varying input sequence
• this is a constrained nonlinear least squares problem:
minimizeN∑
k=1‖uk ‖
2 + γN−1∑k=1
‖uk+1 − uk ‖2
subject to x2 = f (0,u1)
xk+1 = f (xk,uk), k = 2, . . . ,N − 1
xfinal = f (xN,uN)
• variables are u1, . . . ,uN , x2, . . . , xN
Constrained nonlinear least squares 11.26
![Page 32: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/32.jpg)
Example solution trajectories
−0.5 0 0.5
0
1
xfinal = (0,1,0)
−0.5 0 0.5
0
1
xfinal = (0,1, π/2)
Constrained nonlinear least squares 11.27
![Page 33: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/33.jpg)
Example solution trajectories
−0.5 0
0
0.5
xfinal = (0,0.5,0)
0 0.5
0
0.5
xfinal = (0.5,0.5,−π/2)
Constrained nonlinear least squares 11.28
![Page 34: L. Vandenberghe ECE133B (Spring 2020) 11. Constrained ...vandenbe/133B/lectures/nllseq.pdfFirst-order optimality conditions minimize h„x” subject to g„x” = 0 h is a function](https://reader034.vdocuments.site/reader034/viewer/2022051321/5fff39f9126aae10885e0a22/html5/thumbnails/34.jpg)
Inputs for four trajectories
0 10 20 30 40 50
−0.5
0
0.5
Speed
Steering angle
k
u k
0 10 20 30 40 50
−0.5
0
0.5Speed
Steering angle
k
u k
0 10 20 30 40 50
−0.5
0
0.5
Speed
Steering angle
k
u k
0 10 20 30 40 50
−0.5
0
0.5Speed Steering angle
k
u k
Constrained nonlinear least squares 11.29