minimizeᵤ ∑ₜ uₜRₜuₜ + 2 zₜᵀ uₜ + xₜQₜxₜ + 2 sₜᵀ xₜ
s.t. xₜ₊₁ = A xₜ₊₁ + B uₜ
returns Pₜ, oₜᵀ, Kₜ, kₜ
s.t.
optimal uₜ = - Kₜxₜ - kₜ
and
Value function is quadratic
xₜPₜxₜ + 2 oₜᵀ xₜ ≡ ∑ₖ₌ₜᵀ uₖRuₖ + 2 zₖᵀ uₖ + xₖQxₖ + 2 sₖᵀ xₖ
s.t. xₖ₊₁ = A xₖ₊₁ + B uₖ
Source code in bayes_cbf/ilqr.py
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76 | def affine_backpropagation(Q, s, R, z, A, B, P, o, γ=1):
"""
minimizeᵤ ∑ₜ uₜRₜuₜ + 2 zₜᵀ uₜ + xₜQₜxₜ + 2 sₜᵀ xₜ
s.t. xₜ₊₁ = A xₜ₊₁ + B uₜ
returns Pₜ, oₜᵀ, Kₜ, kₜ
s.t.
optimal uₜ = - Kₜxₜ - kₜ
and
Value function is quadratic
xₜPₜxₜ + 2 oₜᵀ xₜ ≡ ∑ₖ₌ₜᵀ uₖRuₖ + 2 zₖᵀ uₖ + xₖQxₖ + 2 sₖᵀ xₖ
s.t. xₖ₊₁ = A xₖ₊₁ + B uₖ
"""
# Solution:
# (1) Kₜ = (R + BᵀPₜ₊₁B)⁻¹BᵀPₜ₊₁Aₜ
# (2) Pₜ = Qₜ + AₜᵀPₜ₊₁Aₜ - AₜᵀPₜ₊₁BKₜ
# (3) oₜ = sₜ + Aₜᵀoₜ₊₁ - Kₜᵀ(zₜ + Bᵀoₜ₊₁)
# (4) kₜ = (R + BᵀPₜ₊₁B)⁻¹(zₜ + Bᵀoₜ₊₁)
# Eq(1)
P = γ*P
o = γ*o
G = R + B.T @ (P) @ (B)
K = torch.solve(B.T @ (P) @ (A), G)[0]
# Eq(2)
P_new = Q + A.T @ (P) @ (A) - A.T @ (P) @ (B) @ (K)
# Eq(3)
o_new = s + A.T @ (o) - K.T @ (z + B.T @ (o))
# Eq(4)
k = torch.solve((z + B.T @ (o)).unsqueeze(-1), G)[0].squeeze(-1)
return P_new, o_new, K, k
|