LQR optimal control of accumulator via dynamic programming

Question

Let

$$ x_{k+1} = x_k + u_k, \qquad x_0 = 5 $$

where $k \in \{ 0,1,2,3 \}$, define the return function

$$ - \sum_{k=0}^3 \left( x_k^2 + u_k^2 \right) $$

with the following inequality constraints on the control inputs

$$0 \leq x_k + u_k \leq 5$$

where $u_k \in \Bbb Z$. I would like to apply dynamic programming to find an optimal control sequence $u_0^*, u_1^*, u_2^*, u_3^*$ that maximizes the return function.

My idea was to convert the maximization problem of the return function into a minimization problem of the cost function and apply backward dynamic programming. However, I do not know how to find the minimized cost at each stage. Can anyone please help?

Answer 1

Problem Restated

The problem is to find the minimum of the cost defined by,

$$\sum_{k=0}^3 x_k^2 + u_k^2$$ with the initial condition $x_0=5$ and relation $0\le x_{k+1}=x_k+u_k\le5$.

Simple Arithmetic

$$ x_0=5\ \stackrel{u_0=-3}{\xrightarrow{\hspace{2em}}}\ x_1=2\ \stackrel{u_1=-1}{\xrightarrow{\hspace{2em}}}\ x_2=1\ \stackrel{u_2=-1}{\xrightarrow{\hspace{2em}}}\ x_3=0\ \stackrel{u_3=0}{\xrightarrow{\hspace{2em}}} $$

The cost of the state transition above is

$$(5^2+(-3)^2) + (2^2+(-1)^2)+ (1^2+(-1)^2) + (0^2+0^2)=5^2+16=41$$

---

Here is the simple arithmetic that shows 41 is the minimum cost. Note that a square number is always non-negative.

1. If $x_1=0,1,4,5$, then $u_0=-5, -4, -1, 0$. $$\ \sum_{k=0}^3 x_k^2 + u_k^2\ge x_0^2+(u_0^2+x_1^2)\ge 5^2 + 17$$
2. If $x_1=3, x_1=2$, then $u_0=-2, -3$. $$\ \sum_{k=0}^3 x_k^2 + u_k^2= 5^2 + 4 + 9 + (u_1^2+x_2^2 + u_2^2 + x_3^2+u_3^2)\\=5^2 + 13 + (u_1^2+x_2^2 + u_2^2 + x_3^2)\ge 5^2 + 13 + 3=5^2+16.$$

Why does the last inequality hold? We know $x_1\ge2$. Among the numbers $u_1, x_2, u_2, x_3$, there are at least 3 numbers that are either $1$ or $-1$, or one of them is bigger than $1$ or smaller than $-1$, i.e., a number whose square is no less than $2^2=4$. $\checkmark$

Tracing the proof above, we can see that the optimal control sequences are

• $u_0^*=-3$, $u_1^*=-1$, $u_2^*=-1$, $u_3^*=0$ (as shown above).
• $u_0^*=-3$, $u_1^*=-1$, $u_2^*=0$, $u_3^*=0$.

Brute-Force Programming

As Yuval observed, it is a short programming exercise to brute-force the $6^4=1296$ possible feasible solutions. For example, here it is in Python.

min_cost = 999999999999999999  # a number too big
x0 = 5
control_sequence = []
for x1 in range(6):
    cost0 = x0 ** 2 + (x1 - x0) ** 2
    for x2 in range(6):
        cost1 = cost0 + x1 ** 2 + (x2 - x1) ** 2
        for x3 in range(6):
            cost2 = cost1 + x2 ** 2 + (x3 - x2) ** 2
            for x4 in range(6):
                cost3 = cost2 + x3 ** 2 + (x4 - x3) ** 2
                if min_cost > cost3:
                    min_cost = cost3
                    control_sequence = [[x1 - x0, x2 - x1, x3 - x2, x4 - x3]]
                elif min_cost == cost3:
                    control_sequence.append([x1 - x0, x2 - x1, x3 - x2, x4 - x3])

print(min_cost)  
# 41

print("Optimal Control sequences: ", control_sequence)  
# [[-3, -1, -1, 0], [-3, -1, 0, 0]]

Dynamic Programming

To use dynamic programming, we need to find many overlapping problems, with optimal structure that will lead to recurrent relations. Here we have the natural general problems in the form of the following.

How can we compute the function of minimum cost below? $$f(s, t, x_0)=\min_{0\le x_{k+1}=x_k+u_k\le s\text{ for all }k}\sum_{k=0}^t x_k^2 + u_k^2$$ where all variables are integers, $0\le x_0\le s$, $0\le t$.

The original problem is the case when $s$ = 5, $x_0=5$ and $t=3$.

Once $x_1$ is chosen, the cost for the remaining state transitions is none other than the cost of $t$ state transitions starting from $x_1$. That is,

$$f(s, t, x_0) =\min_{0\le x_1\le s}\ \left((x_0^2 + (x_1-x_0)^2) + f(s, t-1, x_1)\right)$$

The above recurrent relation enables us to use a nested loop to compute $f$.

1. For $t=0$, $f(s, t, x)=x^2$ for all $x$, $0\le x\le s$.
2. Suppose for some $t$, we have computed $f(s, t-1, x)$ for all $x$, $0\le x\le s$. Use the above recurrence relation to computed $f(s, t, x)$ for all $x$, $0\le x\le s$.

To find the optimal control sequence, keep track of the choices of the variables made for each $f(s,t,x)$ in the loop.

Answer 2

$$ \begin{bmatrix} x_1 \\ x_2 \\ x_3 \\ x_4 \end{bmatrix} = \underbrace{\begin{bmatrix} 1 & 0 & 0 & 0 \\ 1 & 1 & 0 & 0 \\ 1 & 1 & 1 & 0 \\ 1 & 1 & 1 & 1 \end{bmatrix}}_{=: {\bf L}} \begin{bmatrix} u_0 \\ u_1 \\ u_2 \\ u_3 \end{bmatrix} + x_0\begin{bmatrix} 1 \\ 1 \\ 1 \\ 1 \end{bmatrix} $$

or, more succinctly,

$$ {\bf x} = {\bf L} {\bf u} + x_0 {\bf 1}_4 $$

Since the final state cost is zero, we define weight vector ${\bf w} := \begin{bmatrix} 1 & 1 & 1 & 0 \end{bmatrix}^\top$. Hence, the optimal control input sequence can be found via

$$\boxed{\begin{array}{ll} \underset{{\bf u} \in \Bbb Z^4}{\text{minimize}} & x_0^2 + \left\| \mbox{diag}({\bf w}) \left( {\bf L} {\bf u} + x_0 {\bf 1}_4 \right) \right\|_2^2 + \left\| {\bf u} \right\|_2^2 \\ \text{subject to} & {\bf 0}_4 \leq {\bf L} {\bf u} + x_0 {\bf 1}_4 \leq x_0 {\bf 1}_4\end{array}}$$

which is an instance of integer regularized least squares.

Using Python (with NumPy and CVXPY),

import cvxpy as cp
import numpy as np

x_0 = 5

L = np.array([( 1, 0, 0, 0),
              ( 1, 1, 0, 0),
              ( 1, 1, 1, 0),
              ( 1, 1, 1, 1)])

w = np.array([1, 1, 1, 0])
W = np.diag(w)

# optimization variables
u = cp.Variable(4, integer=True)

# build state vector
x = (L @ u) + (x_0 * np.ones(4))

# build cost function
cost = cp.sum_squares(x_0) + cp.sum_squares(W @ x) + cp.sum_squares(u)

# create optimization problem
prob = cp.Problem( cp.Minimize(cost),
                 [ np.zeros(4) <= x, x <= x_0 * np.ones(4) ] )

# solve optimization problem
solution = prob.solve(solver = cp.ECOS_BB)

print("Optimal input sequence: ", np.round(        u.value, 2))
print("Optimal state sequence: ", np.append([x_0], x.value   ))
print("Minimal cost:           ", np.round(     cost.value, 2))

which outputs the following

Optimal input sequence:  [-3. -1.  0. -0.]
Optimal state sequence:  [5. 2. 1. 1. 1.]
Minimal cost:            41.0

Hence, one optimal solution is the following

$$ u_0 = -3, \qquad u_1 = -1, \qquad u_2 = u_3 = 0 $$

Note that this optimal solution is not unique. For details, please consider reading John's answer.