曲率计算

lqr 推导

迭代法求黎卡提（Riccati）方程的解

command

mkdir build
cd build/
cmake ..
make

运行

cd build/
./lqr_controller    //lqr
./lqr_pid_controller  //lqr + pid

结果

LQR (恒速 2m/s)

LQR + PID （初始速度为0 ， 目标速度为4m/s）