lqr ilqr代码实例

lqr 代码实例

import numpy as np
import scipy.linalg

def lqr(A, B, Q, R):
P = np.matrix(scipy.linalg.solve_continuous_are(A, B, Q, R))
K = np.matrix(scipy.linalg.inv(R) * (B.T * P))
return K

# 系统动力学矩阵
A = np.array([[0, 1], [0, -1]])
B = np.array([[0], [1]])

# 代价函数权重矩阵
Q = np.array([[1, 0], [0, 1]])
R = np.array([[1]])

K = lqr(A, B, Q, R)
print("LQR 控制增益：\n", K)

ilqr 代码实例

import numpy as np

def ilqr(A, B, Q, R, N=10, tol=1e-6):
n, m = B.shape
K_list = [np.zeros((m, n)) for _ in range(N)]

for _ in range(1000):
P = Q
K_new_list = []

for i in range(N - 1, -1, -1):
K = -np.linalg.inv(R + B.T @ P @ B) @ B.T @ P @ A
K_new_list.append(K)

P = Q + K.T @ R @ K + (A + B @ K).T @ P @ (A + B @ K)

diff = np.sum([np.linalg.norm(K_new_list[i] - K_list[i]) for i in range(N)])

K_list = K_new_list[::-1]
if diff < tol:
break

return K_list

# 系统动力学矩阵
A = np.array([[0, 1], [0, -1]])
B = np.array([[0], [1]])

# 代价函数权重矩阵
Q = np.array([[1, 0], [0, 1]])
R = np.array([[1]])

K_list = ilqr(A, B, Q, R)
print("iLQR 控制增益序列：")
for i, K in enumerate(K_list):
print(f"K_{i}:\n", K)

lqr ilqr代码实例相关推荐

php 3 3公派算法代码,PHP常见算法合集代码实例
许多人都说算法是程序的核心,一个程序的好于差,关键是这个程序算法的优劣,下面是一些常用的算法和实例,大家可以好好学习下一.文件夹遍历 function allFile($path = __DIR_ ...
asp.net获取页面url参数值的实现代码实例
asp.net获取页面url参数值的实现代码实例 file: default.aspx.cs using system; using system.data; using system.configu ...
feign调用多个服务_Spring Cloud多个微服务之间调用代码实例
这篇文章主要介绍了Spring Cloud多个微服务之间调用代码实例,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友可以参考下现在又一个学生微服务 user ...
多激光雷达外参⾃动化标定算法及代码实例
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达激光雷达是⽬前⾃动驾驶系统中的核⼼传感器之⼀,但是由于其信息密度低.存在垂直盲区等问题,⼚商⼤多在其L ...
php设置背景图片的代码,css设置背景图片如何实现？（代码实例）
本篇文章给大家带来的内容是关于css设置背景图片如何实现?(代码实例),有一定的参考价值,有需要的朋友可以参考一下,希望对你有所帮助. 很多人提交表单时都喜欢用一个图片来作为提交按钮,大多数人可能用J ...
python雷达图数据_PYTHON绘制雷达图代码实例
这篇文章主要介绍了PYTHON绘制雷达图代码实例,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友可以参考下 1.雷达图 import matplotlib.py ...
jQuery实现自动左右滚动效果的代码实例
2019独角兽企业重金招聘Python工程师标准>>> jQuery实现自动左右滚动效果的代码实例思路: 点击滚动模式下,是为点击(向前/向后/数字)添加click事件,通过控制展 ...
java字典写实例,基于JAVA的新华字典接口调用代码实例
基于JAVA的新华字典接口调用代码实例接口描述:基于JA V A的新华字典接口调用代码实例接口平台:聚合数据 import java.io.BufferedReader; import java. ...
php最常见代码,PHP常见算法合集代码实例
许多人都说算法是程序的核心,一个程序的好于差,关键是这个程序算法的优劣,下面是一些常用的算法和实例,大家可以好好学习下一.文件夹遍历<?php function allFile($path ...

lqr ilqr代码实例

lqr ilqr代码实例相关推荐

最新文章

热门文章