//  [python]
def _solve_lqr(self, A, B, Q, R, goals):
        # a differentiable process of solving LQR, 
        # time-invariant A, B, Q, R (with leading batch dimensions) but goals can be a batch of trajectories (batch_size, T+1, k)
        #       min \Sigma^{T} (x_t - goal[t])^T Q (x_t - goal[t]) + u_t^T R u_t
        # s.t.  x_{t+1} = A x_t + B u_t
        # return feedback gain and feedforward terms such that u = -K x + k

        # goals include the terminal reference
        T = self._T

        K = [None] * T
        k = [None] * T

        V = [None] * (T+1)
        v = [None] * (T+1)

        A_trans = A.transpose(-2,-1)
        B_trans = B.transpose(-2,-1)
        #initialization for backpropagation
        V[-1] = Q
        if goals is not None:
            v[-1] = batch_mv(Q, goals[:, -1, :])
            for i in reversed(range(T)):
                # (B^T V B + R)^{-1}
                # V_uu_inv = torch.inverse(
                #     torch.matmul(
                #     torch.matmul(B_trans, V[i+1]),
                #     B
                #     ) + R
                # ) 
                # (B^T V B + R)^{-1} B^T
                # using torch.solve(B, A) to obtain the solution of A X = B to avoid direct inverse, note it also returns LU
                # for new torch.linalg.solve, no LU is returned
                V_uu_inv_B_trans = torch.solve(B_trans, torch.matmul(torch.matmul(B_trans, V[i+1]),
                    B
                    ) + R)[0]
                # V_uu_inv_B_trans = torch.matmul(
                #     torch.inverse(
                #     torch.matmul(
                #     torch.matmul(B_trans, V[i+1]),
                #     B
                #     ) + R
                #     ), B_trans 
                # )
                # K = (B^T V B + R)^{-1} B^T V A 
                K[i] = torch.matmul(
                        torch.matmul(
                            V_uu_inv_B_trans,
                            V[i+1]
                        ),
                        A
                    )

                k[i] = batch_mv(V_uu_inv_B_trans, v[i+1])

                #riccati difference equation
                # A-BK
                A_BK = A - torch.matmul(B, K[i])
                # V = A^T V (A-BK) + Q = A^T V A - A^T V B (B^T V B + R)^{-1} B^T V A + Q
                V[i] = torch.matmul(torch.matmul(A_trans, V[i+1]), A_BK) + Q
                # v = (A-BK)^Tv + Q r
                v[i] = batch_mv(A_BK.transpose(-2, -1), v[i+1]) + batch_mv(Q, goals[:, i, :])
        else:
            #None goals means a fixed regulation point at origin. ignore k and v for efficiency
            for i in reversed(range(T)):
                # using torch.solve(B, A) to obtain the solution of A X = B to avoid direct inverse, note it also returns LU
                V_uu_inv_B_trans = torch.solve(B_trans, torch.matmul(torch.matmul(B_trans, V[i+1]),
                    B
                    ) + R)[0]
                # V_uu_inv_B_trans = torch.matmul(
                #     torch.inverse(
                #     torch.matmul(
                #     torch.matmul(B_trans, V[i+1]),
                #     B
                #     ) + R
                #     ), B_trans 
                # )
                # K = (B^T V B + R)^{-1} B^T V A 
                K[i] = torch.matmul(
                        torch.matmul(
                            V_uu_inv_B_trans,
                            V[i+1]
                        ),
                        A
                    )
                #riccati difference equation
                # A-BK
                A_BK = A - torch.matmul(B, K[i])
                # V = A^T V (A-BK) + Q = A^T V A - A^T V B (B^T V B + R)^{-1} B^T V A + Q
                V[i] = torch.matmul(torch.matmul(A_trans, V[i+1]), A_BK) + Q
            k[:] = self._zero_tensor_constant_k
            v[:] = self._zero_tensor_constant_v       

        # we might need to cat or stack to return them as tensors but for mpc maybe only the first time step is useful...
        # note K is for negative feedback, namely u = -Kx+k
        return K, k, V, v