成本函数依赖于时变LQR返回的K个矩阵的非线性系统的直接转录

我正在努力实现一种名为DIRTREL的轨迹优化算法，该算法本质上是直接转录，具有附加的成本函数。但是，成本函数包含通过围绕决策变量（x，u）线性化系统并采用离散时变LQR获得的K个矩阵。我的问题是，由于Riccati差分方程的递归性质，当我当前的方法以符号方式描述该系统并导致极其冗长的符号方程时（只会随着更多的时间步长而增加长度），因此如何在drake中最有效，简洁地表达这一点，甚至这种象征性的方法是否合适。

更多细节：

1. 将我的系统指定为LeafSystem
2. 用决策变量x，u声明数学程序
3. 要获取随时间变化的线性动力学，请指定一个类，该类在单个时间步上接受动力学和决策变量，并使用symbolic.Jacobian（args）返回该时间步的Jacobian
4. 添加成本函数，该函数将吸收整个轨迹，因此所有x，u

成本函数内部：

1. 通过使用接收决策变量并返回雅可比行列的类，获取每个时间步的线性化矩阵A_i，B_i，G_i（噪声为G_i）
2. 使用Riccati差分方程，使用A_i和B_i矩阵并求解Ks，计算TVLQR成本（S [n]）
3. 返回数学程序的成本，该成本实质上是K个矩阵的大型线性组合

旁注是我不确定以符号形式计算逆的最易处理的方法，但是我最关心的是我的方法论以及这种符号描述是否合适。