User:Vkadakkal

LQR Controller with Preview (i/p depends on prev. i/p)
$$ x_{k+1} = Ax_k + Bu_k + f_k $$

Performance Measure

$$ J = \frac{1}{2} x_N^TMx_N + \frac{1}{2} \Sigma_{k=0}^{N-1} x_k^TQx_k + u_k^TRu_k + u_{k-1}^TRu_{k-1} $$

Augment the system

$$ \left[\begin{array}{c} x_{k+1}\\ v_{k+1} \end{array}\right] = \left[\begin{array}{c c} A & 0\\ 0 & 0 \end{array}\right] \left[\begin{array}{c} x_k\\ v_k \end{array}\right] + \left[\begin{array}{c} B\\ I \end{array}\right] u_k + f_{a_k} $$

$$ x_{a_k} = \left[\begin{array}{c} x_k\\ v_k \end{array}\right] $$

Performance measure is now:

$$ J = \frac{1}{2} x_{a_N}^TQ_{AN}x_{a_N} + \frac{1}{2} \Sigma_{k=0}^{N-1} x_{a_k}^TQ_Ax_

{a_k} + u_k^TMx_{a_k} + u_k^TR_Au_k$$

where

$$ Q_A = \left[\begin{array}{c c} Q & 0\\ 0 & T \end{array}\right] $$,

$$ M = \left[\begin{array}{c c} 0 & -T \end{array}\right] $$

and

$$ R_A = R + T $$

Let $$g[x_{a_k}]$$ be the cost for moving the state from $$x_{a_k}$$ to

$$x_{a_N}$$

$$ g[x_{a_k}] = min \left( \frac{1}{2} x_{ak}^TQ_Ax_k + u_k^TMx_{a_k} + \frac{1}{2}

u_k^TR_Au_k + g[x_{k+1}] \right) $$

Assume it's solution to be

$$ g[x_{a_k}] = \frac{1}{2}x_{a_k}^TW_{N-k}x_{a_k} + x_{a_k}^TV_{N-k} + Z_{N-k} $$

$$ = min \left(\frac{1}{2} x_{a_k}^TQ_Ax_{a_k} + 		u_k^TMx_{a_k} + 		\frac{1}{2} u_k^TR_Au_k + 		\frac{1}{2}x_{a_{k+1}}^TW_{N-k-1}x_{a_{k+1}} + 		x_{a_{k+1}}^TV_{N-k-1} + Z_{N-k-1} \right) $$

$$ = min \left(\frac{1}{2}x_{a_k}^T[Q_A + A_A^TW_{N-k-1}A_A]x_{a_k} + 		\frac{1}{2}u_k^T[R_A + B_A^TW_{N-k-1}B_A]u_k \right)+ ... $$ $$...\left(		u_k^T(Mx_{a_k} + B_A^TW_{N-k-1}A_Ax_{a_k} + B_A^TV_{N-k-1} + B_A^TW_{N-k-1}f_{a_k}) + \right)... $$ $$...\left(		x_{a_k}^T(A_A^TW_{N-k-1}f_{a_k} + A_A^TV_{N-k-1}) +		f_{a_k}^TV_{N-k-1} + \frac{1}{2}f_{a_k}^TW_{N-k-1}f_{a_k} + 		Z_{N-k-1} \right) $$

Differentiating w.r.t $$u_k$$ and equating it to 0, the optimal control

$$u_k^*$$ is found to be:

$$ u_k^* = -[R_A + B_A^TW_{N-k-1}B_A]^{-1} \left[ (M + B_A^TW_{N-k-1}A_A)x_{a_k} + B_A^TV_{N-k-1} + B_A^TW_{N-k-1}f_{a_k}	\right] $$

Let

$$u_k^* = -(U_v + K_x x_{a_k} + K_f f_{a_k})$$

where,

$$ S = [R_A + B_A^TW_{N-k-1}B_A]^{-1}$$

$$ U_v = SB_A^TV_{N-k-1} $$

$$ K_x = S(M + B_A^TW_{N-k-1}A_A) $$

$$ K_f = SB_A^TW_{N-k-1} $$

Feeding $$u_k^*$$ back into the expression for $$g[x_{a_k}]$$, we

get:

$$ RHS = 	\frac{1}{2}x_{a_k}^T[Q_A + A_A^TW_{N-k-1}A_A]x_{a_k} + \frac{1}{2}u_k^TS^{-1}u_k - u_k^TS^{-1}u_k + x_{a_k}^T [A_A^TW_{N-k-1}f_{a_k} + A_A^TV_{N-k-1}] +... $$

$$	f_{a_k}^TV + \frac{1}{2}f_k^TWf_k + Z $$

$$ = \frac{1}{2}x_{a_k}^T[Q_A + A_A^TW_{N-k-1}A_A - K_x^TS^{-1}K_x]x_{a_k} + x_{a_k}^T [A_A^TW_{N-k-1}f_{a_k} + A_A^TV_{N-k-1} - \frac{1}{2}K_x^TS^{-1}(U_v + K_f f_{a_k})] +... $$

$$ U_v^TS^{-1}(U_v + K_x x_{a_k} + K_f f_{a_k}) + other terms $$

Grouping terms in the format for $$g[x_k]$$ and comparing, we get:

$$ W_{N-k} = Q_A + A_A^TW_{N-k-1}A_A + K_x^TS^{-1}K_x $$

$$ V_{N-k} = A_A^TW_{N-k-1}f_{a_k} + A_A^TV_{N-k-1} - \frac{1}{2}K_x^TS^{-1}(U_v + K_f f_{a_k}) $$

$$Z_{N-k} = $$ All Other terms...

$$V_{N-k}$$ and $$W_{N-k}$$ are solved iteratively. $$Z_{N-k}

$$ is not needed for computation of control $$u_k^*$$, so is left

untouched!

LQR Controller with Preview
$$ x_{k+1} = Ax_k + Bu_k + f_k $$

Performance Measure

$$ J = \frac{1}{2} x_N^TMx_N + \frac{1}{2} \Sigma_{k=0}^{N-1} x_k^TQx_k + u_k^TRu_k $$

Let $$g[x_k]$$ be the cost for moving the state from $$x_k$$ to

$$x_N$$

$$ g[x_k] = min \left( \frac{1}{2} x_k^TQx_k + \frac{1}{2} u_k^TRu_k + g[x_{k+1}] \right) $$

Assume it's solution to be

$$ g[x_k] = \frac{1}{2}x_k^TW_{N-k}x_k + x_k^TV_{N-k} + Z_{N-k} $$

$$ = min \left(\frac{1}{2} x_k^TQx_k + 		\frac{1}{2} u_k^TRu_k + 		\frac{1}{2}x_{k+1}^TW_{N-k-1}x_{k+1} + 		x_{k+1}^TV_{N-k-1} + Z_{N-k-1} \right) $$

$$ = min \left(\frac{1}{2}x_k^T[Q + A^TW_{N-k-1}A]x_k + 		\frac{1}{2}u_k^T[R + B^TW_{N-k-1}B]u_k \right)+ ... $$ $$...\left(		u_k^T(B^TW_{N-k-1}Ax_k + B^TV_{N-k-1} + B^TW_{N-k-1}f_k) + 		x_k^T(A^TW_{N-k-1}f_k + A^TV_{N-k-1}) \right)+ ... $$ $$...\left(		f_k^TV_{N-k-1} + \frac{1}{2}f_k^TW_{N-k-1}f_k + 		Z_{N-k-1} \right) $$

Differentiating w.r.t $$u_k$$ and equating it to 0, the optimal control

$$u_k^*$$ is found to be:

$$ u_k^* = -[R + B^TW_{N-k-1}B]^{-1}B^T \left[ V_{N-k-1} + W_{N-k-1}Ax_k + W_{N-k-1}f_k

\right] $$

Let

$$u_k^* = -(U_v + K_x x_k + K_f f_k)$$

where,

$$ S = [R + B^TW_{N-k-1}B]^{-1}$$

$$ U_v = SB^TV_{N-k-1} $$

$$ K_x = SB^TW_{N-k-1}A $$

$$ K_f = SB^TW_{N-k-1} $$

Feeding $$u_k^*$$ back into the expression for $$g[x_k]$$, we get:

$$ RHS =	\frac{1}{2}x_k^T[Q+A^TWA]x_k + \frac{1}{2}[U_v^T + x_k^TK_x^T + f_k^TK_f^T][R + B^TWB][U_v + K_x x_k + K_f f_k]+ ...$$ $$ 	- [U_v^T + x_k^TK_x^T + f_k^TK_f^T][B^TWAx_k + B^TV + B^TWf_k] + ... $$ $$ x_k^T[A^TWf_k + A^TV] + f_K^TV + \frac{1}{2}f_k^TWf_k + Z $$

Grouping terms in the format for $$g[x_k]$$ and comparing, we get:

$$ W_{N-k} = Q + A^TW_{N-k-1}A + K_x^T[R+B^TW_{N-k-1}B]K_x - 2K_x^TB^TW_{N-k-1}A $$

But $$K_x^TB^TW_{N-k-1}A = K_x^TB^TS^{-1}(SW_{N-k-1}A) = K_x^TS^{-1}K_x $$

So

$$ W_{N-k} = Q + A^TW_{N-k-1}A - K_x^TS^{-1}K_x $$

$$ V_{N-k} = A^TW_{N-k-1}f_k + A^TV_{N-k-1} - K_x^TB^T(V_{N-k-1} + W_{N-k-1}f_k) $$

$$Z_{N-k} = $$ All Other terms...

$$V_{N-k}$$ and $$W_{N-k}$$ are solved iteratively. $$Z_{N-k}

$$ is not needed for computation of control $$u_k^*$$, so is left

untouched!