根据上述迭代法求解P，P为Riccati方程的解，然而用LQR需要计算K，再将K算出。
（迭代过程中 ，我们可以将此算法和dlqr函数求解的参数进行对比，当误差小于我们设置的允许误差我们就可以把此算法替换掉dlar函数）
今天我又把离散和连续混在一起了，以后要万分注意，避免bug
根据上述迭代法求解P，P为Riccati方程的解，然而用LQR需要计算K，再将K算出。
（迭代过程中 ，我们可以将此算法和dlqr函数求解的参数进行对比，当误差小于我们设置的允许误差我们就可以把此算法替换掉dlar函数）
今天我又把离散和连续混在一起了，以后要万分注意，避免bug