add LQR planning

Author: AtsushiSakai

README.md

@@ -5,7 +5,6 @@
 
 Python codes for robotics algorithm.
 
-
 # Table of Contents
    * [What is this?](#what-is-this)
    * [Requirements](#requirements)
@@ -53,6 +52,7 @@ Python codes for robotics algorithm.
       * [Quintic polynomials planning](#quintic-polynomials-planning)
       * [Dubins path planning](#dubins-path-planning)
       * [Reeds Shepp planning](#reeds-shepp-planning)
+      * [LQR based path planning](#lqr-based-path-planning)
       * [Optimal Trajectory in a Frenet Frame](#optimal-trajectory-in-a-frenet-frame)
    * [Path tracking](#path-tracking)
       * [Pure pursuit tracking](#pure-pursuit-tracking)
@@ -546,6 +546,13 @@ Ref:
 - [ghliu/pyReedsShepp: Implementation of Reeds Shepp curve\.](https://github.com/ghliu/pyReedsShepp)
 
 
+## LQR based path planning
+
+A sample code using LQR based path planning for double itegrator model.
+
+![RSPlanning](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true)
+
+
 ## Optimal Trajectory in a Frenet Frame 
 
 ![3](https://github.com/AtsushiSakai/PythonRobotics/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif)
@@ -671,3 +678,4 @@ You can support financially this project via PayPal.
 
 
 
+

tests/test_LQR_planner.py

@@ -0,0 +1,15 @@
+from unittest import TestCase
+
+import sys
+sys.path.append("./PathPlanning/LQRPlanner")
+
+from PathPlanning.LQRPlanner import LQRPlanner as m
+
+print(__file__)
+
+
+class Test(TestCase):
+
+    def test1(self):
+        m.show_animation = False
+        m.main()