update docs

docs/modules/Kalmanfilter_basics.rst

@@ -1,9 +1,12 @@
 
-# *KF Basics - Part I*
-----------------------
+KF Basics - Part I
+------------------
+
+Introduction
+~~~~~~~~~~~~
 
 What is the need to describe belief in terms of PDF’s?
-''''''''''''''''''''''''''''''''''''''''''''''''''''''
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 This is because robot environments are stochastic. A robot environment
 may have cows with Tesla by side. That is a robot and it’s environment
@@ -14,7 +17,7 @@ Hence, we always have to model around some mean and variances
 associated.
 
 What is Expectation of a Random Variables?
-''''''''''''''''''''''''''''''''''''''''''
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 Expectation is nothing but an average of the probabilites
 
@@ -40,17 +43,17 @@ In the continous form,
 
 
 What is the advantage of representing the belief as a unimodal as opposed to multimodal?
-''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 Obviously, it makes sense because we can’t multiple probabilities to a
 car moving for two locations. This would be too confusing and the
 information will not be useful.
 
-## Variance, Covariance and Correlation
----------------------------------------
+Variance, Covariance and Correlation
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 Variance
-~~~~~~~~
+^^^^^^^^
 
 Variance is the spread of the data. The mean does’nt tell much **about**
 the data. Therefore the variance tells us about the **story** about the
@@ -73,7 +76,7 @@ data meaning the spread of the data.
 
 
 Covariance
-~~~~~~~~~~
+^^^^^^^^^^
 
 This is for a multivariate distribution. For example, a robot in 2-D
 space can take values in both x and y. To describe them, a normal
@@ -170,8 +173,8 @@ Covariance taking the data as **population** with :math:`\frac{1}{N}`
 
 
 
-# Gaussians
------------
+Gaussians
+~~~~~~~~~
 
 Central Limit Theorem
 ^^^^^^^^^^^^^^^^^^^^^
@@ -204,11 +207,11 @@ increase the sample size.(Generally, for n>=30)
 
 
 
-.. image:: /Users/atsushisakai/Dropbox/Program/python/PythonRobotics/docs/modules/Localization/Kalmanfilter_basics_14_1.png
+.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png
 
 
 Gaussian Distribution
-'''''''''''''''''''''
+^^^^^^^^^^^^^^^^^^^^^
 
 A Gaussian is a *continuous probability distribution* that is completely
 described with two parameters, the mean (:math:`\mu`) and the variance
@@ -241,11 +244,11 @@ charecteristic **bell curve**.
 
 
 
-.. image:: /Users/atsushisakai/Dropbox/Program/python/PythonRobotics/docs/modules/Localization/Kalmanfilter_basics_16_0.png
+.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png
 
 
 Why do we need Gaussian distributions?
-''''''''''''''''''''''''''''''''''''''
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
 Since it becomes really difficult in the real world to deal with
 multimodal distribution as we cannot put the belief in two seperate
@@ -254,8 +257,8 @@ impossible to comprehend. Gaussian probability distribution allows us to
 drive the robots using only one mode with peak at the mean with some
 variance.
 
-## Gaussian Properties
-----------------------
+Gaussian Properties
+~~~~~~~~~~~~~~~~~~~
 
 **Multiplication**
 
@@ -318,7 +321,7 @@ New mean is
 
 
 
-.. image:: /Users/atsushisakai/Dropbox/Program/python/PythonRobotics/docs/modules/Localization/Kalmanfilter_basics_19_1.png
+.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png
 
 
 **Addition**
@@ -369,7 +372,7 @@ of the two.
 
 
 
-.. image:: /Users/atsushisakai/Dropbox/Program/python/PythonRobotics/docs/modules/Localization/Kalmanfilter_basics_21_1.png
+.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png
 
 
 .. code:: ipython3
@@ -435,7 +438,7 @@ of the two.
 
 
 
-.. image:: /Users/atsushisakai/Dropbox/Program/python/PythonRobotics/docs/modules/Localization/Kalmanfilter_basics_22_0.png
+.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png
 
 
 This is a 3D projection of the gaussians involved with the lower surface
@@ -443,8 +446,7 @@ showing the 2D projection of the 3D projection above. The innermost
 ellipse represents the highest peak, that is the maximum probability for
 a given (X,Y) value.
 
-numpy einsum examples
-'''''''''''''''''''''
+\*\* numpy einsum examples \*\*
 
 .. code:: ipython3
 
@@ -545,7 +547,7 @@ numpy einsum examples
 
 
 
-.. image:: /Users/atsushisakai/Dropbox/Program/python/PythonRobotics/docs/modules/Localization/Kalmanfilter_basics_28_1.png
+.. image:: Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png
 
 
 References:

docs/modules/Kalmanfilter_basics_2.rst

@@ -1,16 +1,12 @@
 
-# *KF Basics - Part 2*
-----------------------
+KF Basics - Part 2
+------------------
 
-+-----------------------------------------------------------------------+
-| ### Probabilistic Generative Laws                                     |
-+=======================================================================+
-| \*1st Law**:                                                          |
-+-----------------------------------------------------------------------+
-| he belief representing the state :math:`x_{t}`, is conditioned on all |
-| past states, measurements and controls. This can be shown             |
-| mathematically by the conditional probability shown below:            |
-+-----------------------------------------------------------------------+
+### Probabilistic Generative Laws
+
+**1st Law**: The belief representing the state :math:`x_{t}`, is
+conditioned on all past states, measurements and controls. This can be
+shown mathematically by the conditional probability shown below:
 
 .. math:: p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})
 
@@ -53,8 +49,8 @@ Probability**
 :math:`\bullet` $p(z_{t} \| x_{t})
 :raw-latex:`\rightarrow `$\ **Measurement Probability**
 
-### Conditional dependence and independence example:
-----------------------------------------------------
+Conditional dependence and independence example:
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 :math:`\bullet`\ **Independent but conditionally dependent**
 
@@ -88,8 +84,8 @@ than Coin 1. This in turn increases the conditional probability that B
 occurs. This suggests that A and B are not independent. On the other
 hand, given C (Coin 1 is selected), A and B are independent.
 
-### Bayes Rule:
----------------
+Bayes Rule:
+~~~~~~~~~~~
 
 Posterior = $:raw-latex:`\frac{Likelihood*Prior}{Marginal}` $
 
@@ -132,8 +128,8 @@ had cancer (A). This is the chance of a true positive, 80% in our case.
 :math:`\bullet`\ Pr(X|not A) = Chance of a positive test (X) given that
 you didn’t have cancer (~A). This is a false positive, 9.6% in our case.
 
-### Bayes Filter Algorithm
---------------------------
+Bayes Filter Algorithm
+~~~~~~~~~~~~~~~~~~~~~~
 
 The basic filter algorithm is:
 
@@ -162,8 +158,21 @@ information here as the gaussians get multiplied here. (Multiplication
 of gaussian values allows the resultant to lie in between these numbers
 and the resultant covariance is smaller.
 
-### Bayes filter localization example:
---------------------------------------
+Bayes filter localization example:
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. code:: ipython3
+
+    from IPython.display import Image
+    Image(filename="bayes_filter.png",width=400)
+
+
+
+
+.. image:: Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png
+   :width: 400px
+
+
 
 Given - A robot with a sensor to detect doorways along a hallway. Also,
 the robot knows how the hallway looks like but doesn’t know where it is
@@ -199,8 +208,8 @@ in the map.
 Do note that the robot knows the map but doesn’t know where exactly it
 is on the map.
 
-### Bayes and Kalman filter structure
--------------------------------------
+Bayes and Kalman filter structure
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 The basic structure and the concept remains the same as bayes filter for
 Kalman. The only key difference is the mathematical representation of
@@ -229,8 +238,8 @@ or acceleration.
 
 :math:`\ast` denotes *convolution*.
 
-### Kalman Gain
----------------
+Kalman Gain
+~~~~~~~~~~~
 
 .. math::  x = (\mathcal L \bar x)
 
@@ -279,8 +288,8 @@ The variance in terms of the Kalman gain:
 is a scaling term that chooses a value partway between :math:`\mu_z` and
 :math:`\bar\mu`.**
 
-### Kalman Filter - Univariate and Multivariate
------------------------------------------------
+Kalman Filter - Univariate and Multivariate
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 \ **Prediction**\ 
 
@@ -318,8 +327,8 @@ are vectors and matrices, but the concepts are exactly the same:
 -  Use the process model to predict the next state (the prior)
 -  Form an estimate part way between the measurement and the prior
 
-### References:
----------------
+References:
+~~~~~~~~~~~
 
 1. Roger Labbe’s
    `repo <https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python>`__

docs/modules/Planar_Two_Link_IK.rst

@@ -0,0 +1,352 @@
+
+Two joint arm to point control
+------------------------------
+
+.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif
+   :alt: TwoJointArmToPointControl
+
+   TwoJointArmToPointControl
+
+This is two joint arm to a point control simulation.
+
+This is a interactive simulation.
+
+You can set the goal position of the end effector with left-click on the
+ploting area.
+
+Inverse Kinematics for a Planar Two-Link Robotic Arm
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+A classic problem with robotic arms is getting the end-effector, the
+mechanism at the end of the arm responsible for manipulating the
+environment, to where you need it to be. Maybe the end-effector is a
+gripper and maybe you want to pick up an object and maybe you know where
+that object is relative to the robot - but you cannot tell the
+end-effector where to go directly. Instead, you have to determine the
+joint angles that get the end-effector to where you want it to be. This
+problem is known as inverse kinematics.
+
+Credit for this solution goes to:
+https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-geometry/
+
+First, let’s define a class to make plotting our arm easier.
+
+.. code:: ipython3
+
+    %matplotlib inline
+    from math import cos, sin
+    import numpy as np
+    import matplotlib.pyplot as plt
+    
+    class TwoLinkArm:
+        def __init__(self, joint_angles=[0, 0]):
+            self.shoulder = np.array([0, 0])
+            self.link_lengths = [1, 1]
+            self.update_joints(joint_angles)
+            
+        def update_joints(self, joint_angles):
+            self.joint_angles = joint_angles
+            self.forward_kinematics()
+            
+        def forward_kinematics(self):
+            theta0 = self.joint_angles[0]
+            theta1 = self.joint_angles[1]
+            l0 = self.link_lengths[0]
+            l1 = self.link_lengths[1]
+            self.elbow = self.shoulder + np.array([l0*cos(theta0), l0*sin(theta0)])
+            self.wrist = self.elbow + np.array([l1*cos(theta0 + theta1), l1*sin(theta0 + theta1)])
+            
+        def plot(self):
+            plt.plot([self.shoulder[0], self.elbow[0]],
+                     [self.shoulder[1], self.elbow[1]],
+                     'r-')
+            plt.plot([self.elbow[0], self.wrist[0]],
+                     [self.elbow[1], self.wrist[1]],
+                     'r-')
+            plt.plot(self.shoulder[0], self.shoulder[1], 'ko')
+            plt.plot(self.elbow[0], self.elbow[1], 'ko')
+            plt.plot(self.wrist[0], self.wrist[1], 'ko')
+
+Let’s also define a function to make it easier to draw an angle on our
+diagram.
+
+.. code:: ipython3
+
+    from math import sqrt
+    
+    def transform_points(points, theta, origin):
+        T = np.array([[cos(theta), -sin(theta), origin[0]],
+                      [sin(theta), cos(theta), origin[1]],
+                      [0, 0, 1]])
+        return np.matmul(T, np.array(points))
+    
+    def draw_angle(angle, offset=0, origin=[0, 0], r=0.5, n_points=100):
+            x_start = r*cos(angle)
+            x_end = r
+            dx = (x_end - x_start)/(n_points-1)
+            coords = [[0 for _ in range(n_points)] for _ in range(3)]
+            x = x_start
+            for i in range(n_points-1):
+                y = sqrt(r**2 - x**2)
+                coords[0][i] = x
+                coords[1][i] = y
+                coords[2][i] = 1
+                x += dx
+            coords[0][-1] = r
+            coords[2][-1] = 1
+            coords = transform_points(coords, offset, origin)
+            plt.plot(coords[0], coords[1], 'k-')
+
+Okay, we now have a TwoLinkArm class to help us draw the arm, which
+we’ll do several times during our derivation. Notice there is a method
+called forward_kinematics - forward kinematics specifies the
+end-effector position given the joint angles and link lengths. Forward
+kinematics is easier than inverse kinematics.
+
+.. code:: ipython3
+
+    arm = TwoLinkArm()
+    
+    theta0 = 0.5
+    theta1 = 1
+    
+    arm.update_joints([theta0, theta1])
+    arm.plot()
+    
+    def label_diagram():
+        plt.plot([0, 0.5], [0, 0], 'k--')
+        plt.plot([arm.elbow[0], arm.elbow[0]+0.5*cos(theta0)],
+                 [arm.elbow[1], arm.elbow[1]+0.5*sin(theta0)],
+                 'k--')
+        
+        draw_angle(theta0, r=0.25)
+        draw_angle(theta1, offset=theta0, origin=[arm.elbow[0], arm.elbow[1]], r=0.25)
+        
+        plt.annotate("$l_0$", xy=(0.5, 0.4), size=15, color="r")
+        plt.annotate("$l_1$", xy=(0.8, 1), size=15, color="r")
+        
+        plt.annotate(r"$\theta_0$", xy=(0.35, 0.05), size=15)
+        plt.annotate(r"$\theta_1$", xy=(1, 0.8), size=15)
+    
+    label_diagram()
+    
+    plt.annotate("Shoulder", xy=(arm.shoulder[0], arm.shoulder[1]), xytext=(0.15, 0.5),
+        arrowprops=dict(facecolor='black', shrink=0.05))
+    plt.annotate("Elbow", xy=(arm.elbow[0], arm.elbow[1]), xytext=(1.25, 0.25),
+        arrowprops=dict(facecolor='black', shrink=0.05))
+    plt.annotate("Wrist", xy=(arm.wrist[0], arm.wrist[1]), xytext=(1, 1.75),
+        arrowprops=dict(facecolor='black', shrink=0.05))
+    
+    plt.axis("equal")
+    
+    plt.show()
+
+
+
+.. image:: Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png
+
+
+It’s common to name arm joints anatomically, hence the names shoulder,
+elbow, and wrist. In this example, the wrist is not itself a joint, but
+we can consider it to be our end-effector. If we constrain the shoulder
+to the origin, we can write the forward kinematics for the elbow and the
+wrist.
+
+| :math:`elbow_x = l_0\cos(\theta_0)`
+| :math:`elbow_y = l_0\sin(\theta_0)`
+
+| :math:`wrist_x = elbow_x + l_1\cos(\theta_0+\theta_1) = l_0\cos(\theta_0) + l_1\cos(\theta_0+\theta_1)`
+| :math:`wrist_y = elbow_y + l_1\sin(\theta_0+\theta_1) = l_0\sin(\theta_0) + l_1\sin(\theta_0+\theta_1)`
+
+Since the wrist is our end-effector, let’s just call its coordinates
+\ :math:`x`\  and \ :math:`y`\ . The forward kinematics for our
+end-effector is then
+
+| :math:`x = l_0\cos(\theta_0) + l_1\cos(\theta_0+\theta_1)`
+| :math:`y = l_0\sin(\theta_0) + l_1\sin(\theta_0+\theta_1)`
+
+A first attempt to find the joint angles :math:`\theta_0` and
+:math:`\theta_1` that would get our end-effector to the desired
+coordinates :math:`x` and :math:`y` might be to try solving the forward
+kinematics for :math:`\theta_0` and :math:`\theta_1`, but that would be
+the wrong move. An easier path involves going back to the geometry of
+the arm.
+
+.. code:: ipython3
+
+    from math import pi
+    
+    arm.plot()
+    label_diagram()
+    
+    plt.plot([0, arm.wrist[0]],
+             [0, arm.wrist[1]],
+             'k--')
+    
+    plt.plot([arm.wrist[0], arm.wrist[0]],
+             [0, arm.wrist[1]],
+             'b--')
+    plt.plot([0, arm.wrist[0]],
+             [0, 0],
+             'b--')
+    
+    plt.annotate("$x$", xy=(0.6, 0.05), size=15, color="b")
+    plt.annotate("$y$", xy=(1, 0.2), size=15, color="b")
+    plt.annotate("$r$", xy=(0.45, 0.9), size=15)
+    plt.annotate(r"$\alpha$", xy=(0.75, 0.6), size=15)
+    
+    alpha = pi-theta1
+    draw_angle(alpha, offset=theta0+theta1, origin=[arm.elbow[0], arm.elbow[1]], r=0.1)
+    
+    plt.axis("equal")
+    plt.show()
+
+
+
+.. image:: Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png
+
+
+The distance from the end-effector to the robot base (shoulder joint) is
+:math:`r` and can be written in terms of the end-effector position using
+the Pythagorean Theorem.
+
+:math:`r^2` = :math:`x^2 + y^2`
+
+Then, by the law of cosines, :math:`r`\ 2 can also be written as:
+
+:math:`r^2` = :math:`l_0^2 + l_1^2 - 2l_0l_1\cos(\alpha)`
+
+Because :math:`\alpha` can be written as :math:`\pi - \theta_1`, we can
+relate the desired end-effector position to one of our joint angles,
+:math:`\theta_1`.
+
+:math:`x^2 + y^2` = :math:`l_0^2 + l_1^2 - 2l_0l_1\cos(\alpha)`
+
+:math:`x^2 + y^2` = :math:`l_0^2 + l_1^2 - 2l_0l_1\cos(\pi-\theta_1)`
+
+:math:`2l_0l_1\cos(\pi-\theta_1) = l_0^2 + l_1^2 - x^2 - y^2`
+
+| :math:`\cos(\pi-\theta_1) = \frac{l_0^2 + l_1^2 - x^2 - y^2}{2l_0l_1}`
+| :math:`~`
+| :math:`~`
+| :math:`\cos(\pi-\theta_1) = -cos(\theta_1)` is a trigonometric
+  identity, so we can also write
+
+:math:`\cos(\theta_1) = \frac{x^2 + y^2 - l_0^2 - l_1^2}{2l_0l_1}`
+
+which leads us to an equation for :math:`\theta_1` in terms of the link
+lengths and the desired end-effector position!
+
+:math:`\theta_1 = \cos^{-1}(\frac{x^2 + y^2 - l_0^2 - l_1^2}{2l_0l_1})`
+
+This is actually one of two possible solutions for :math:`\theta_1`, but
+we’ll ignore the other possibility for now. This solution will lead us
+to the “arm-down” configuration of the arm, which is what’s shown in the
+diagram. Now we’ll derive an equation for :math:`\theta_0` that depends
+on this value of :math:`\theta_1`.
+
+.. code:: ipython3
+
+    from math import atan2
+    
+    arm.plot()
+    plt.plot([0, arm.wrist[0]],
+             [0, arm.wrist[1]],
+             'k--')
+    
+    p = 1 + cos(theta1)
+    plt.plot([arm.elbow[0], p*cos(theta0)],
+             [arm.elbow[1], p*sin(theta0)],
+             'b--', linewidth=5)
+    plt.plot([arm.wrist[0], p*cos(theta0)],
+             [arm.wrist[1], p*sin(theta0)],
+             'b--', linewidth=5)
+    
+    beta = atan2(arm.wrist[1], arm.wrist[0])-theta0
+    draw_angle(beta, offset=theta0, r=0.45)
+    
+    plt.annotate(r"$\beta$", xy=(0.35, 0.35), size=15)
+    plt.annotate("$r$", xy=(0.45, 0.9), size=15)
+    plt.annotate(r"$l_1sin(\theta_1)$",xy=(1.25, 1.1), size=15, color="b")
+    plt.annotate(r"$l_1cos(\theta_1)$",xy=(1.1, 0.4), size=15, color="b")
+    
+    label_diagram()
+    
+    plt.axis("equal")
+    
+    plt.show()
+
+
+
+.. image:: Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png
+
+
+Consider the angle between the displacement vector :math:`r` and the
+first link :math:`l_0`; let’s call it :math:`\beta`. If we extend the
+first link to include the component of the second link in the same
+direction as the first, we form a right triangle with components
+:math:`l_0+l_1cos(\theta_1)` and :math:`l_1sin(\theta_1)`, allowing us
+to express :math:`\beta` as
+
+:math:`\beta = \tan^{-1}(\frac{l_1\sin(\theta_1)}{l_0+l_1\cos(\theta_1)})`
+
+We now have an expression for this angle :math:`\beta` in terms of one
+of our arm’s joint angles. Now, can we relate :math:`\beta` to
+:math:`\theta_0`? Yes!
+
+.. code:: ipython3
+
+    arm.plot()
+    label_diagram()
+    plt.plot([0, arm.wrist[0]],
+             [0, arm.wrist[1]],
+             'k--')
+    
+    plt.plot([arm.wrist[0], arm.wrist[0]],
+             [0, arm.wrist[1]],
+             'b--')
+    plt.plot([0, arm.wrist[0]],
+             [0, 0],
+             'b--')
+    
+    gamma = atan2(arm.wrist[1], arm.wrist[0])
+    draw_angle(beta, offset=theta0, r=0.2)
+    draw_angle(gamma, r=0.6)
+    
+    plt.annotate("$x$", xy=(0.7, 0.05), size=15, color="b")
+    plt.annotate("$y$", xy=(1, 0.2), size=15, color="b")
+    plt.annotate(r"$\beta$", xy=(0.2, 0.2), size=15)
+    plt.annotate(r"$\gamma$", xy=(0.6, 0.2), size=15)
+    
+    plt.axis("equal")
+    plt.show()
+
+
+
+.. image:: Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png
+
+
+Our first joint angle :math:`\theta_0` added to :math:`\beta` gives us
+the angle between the positive :math:`x`-axis and the displacement
+vector :math:`r`; let’s call this angle :math:`\gamma`.
+
+:math:`\gamma = \theta_0+\beta`
+
+:math:`\theta_0`, our remaining joint angle, can then be expressed as
+
+:math:`\theta_0 = \gamma-\beta`
+
+We already know :math:`\beta`. :math:`\gamma` is simply the inverse
+tangent of :math:`\frac{y}{x}`, so we have an equation of
+:math:`\theta_0`!
+
+:math:`\theta_0 = \tan^{-1}(\frac{y}{x})-\tan^{-1}(\frac{l_1\sin(\theta_1)}{l_0+l_1\cos(\theta_1)})`
+
+We now have the inverse kinematics for a planar two-link robotic arm. If
+you’re planning on implementing this in a programming language, it’s
+best to use the atan2 function, which is included in most math libraries
+and correctly accounts for the signs of :math:`y` and :math:`x`. Notice
+that :math:`\theta_1` must be calculated before :math:`\theta_0`.
+
+| :math:`\theta_1 = \cos^{-1}(\frac{x^2 + y^2 - l_0^2 - l_1^2}{2l_0l_1})`
+| :math:`\theta_0 = atan2(y, x)-atan2(l_1\sin(\theta_1), l_0+l_1\cos(\theta_1))`

docs/modules/appendix.rst

@@ -0,0 +1,9 @@
+.. _appendix:
+
+Appendix
+==============
+
+.. include:: Kalmanfilter_basics.rst
+
+.. include:: Kalmanfilter_basics_2.rst
+

docs/modules/arm_navigation.rst

@@ -3,19 +3,8 @@
 Arm Navigation
 ==============
 
-Two joint arm to point control
-------------------------------
+.. include:: Planar_Two_Link_IK.rst
 
-Two joint arm to a point control simulation.
-
-This is a interactive simulation.
-
-You can set the goal position of the end effector with left-click on the
-ploting area.
-
-Check this doc for machematical background: `Jupyter notebook documentation <https://github.com/AtsushiSakai/PythonRobotics/blob/master/ArmNavigation/two_joint_arm_to_point_control/Planar_Two_Link_IK.ipynb>`_
-
-|arm_to_point|
 
 N joint arm to point control
 ----------------------------
@@ -38,6 +27,5 @@ Arm navigation with obstacle avoidance simulation.
 
 |obstacle|
 
-.. |arm_to_point| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif
 .. |n_joints_arm| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif
 .. |obstacle| image:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif

docs/modules/extended_kalman_filter_localization.rst

@@ -0,0 +1,29 @@
+
+Extended Kalman Filter Localization
+-----------------------------------
+
+.. figure:: https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/extended_kalman_filter/animation.gif
+   :alt: EKF
+
+   EKF
+
+This is a sensor fusion localization with Extended Kalman Filter(EKF).
+
+The blue line is true trajectory, the black line is dead reckoning
+trajectory,
+
+the green point is positioning observation (ex. GPS), and the red line
+is estimated trajectory with EKF.
+
+The red ellipse is estimated covariance ellipse with EKF.
+
+Code; `PythonRobotics/extended_kalman_filter.py at master ·
+AtsushiSakai/PythonRobotics <https://github.com/AtsushiSakai/PythonRobotics/blob/master/Localization/extended_kalman_filter/extended_kalman_filter.py>`__
+
+Kalman Filter
+~~~~~~~~~~~~~
+
+Ref
+~~~
+
+-  `PROBABILISTIC-ROBOTICS.ORG <http://www.probabilistic-robotics.org/>`__

docs/modules/localization.rst

@@ -3,26 +3,8 @@
 Localization
 ============
 
-Extended Kalman Filter localization
------------------------------------
+.. include:: extended_kalman_filter_localization.rst
 
-.. raw:: html
-
-   <img src="https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/extended_kalman_filter/animation.gif" width="640">
-
-This is a sensor fusion localization with Extended Kalman Filter(EKF).
-
-The blue line is true trajectory, the black line is dead reckoning
-trajectory,
-
-the green point is positioning observation (ex. GPS), and the red line
-is estimated trajectory with EKF.
-
-The red ellipse is estimated covariance ellipse with EKF.
-
-Ref:
-
--  `PROBABILISTIC ROBOTICS`_
 
 Unscented Kalman Filter localization
 ------------------------------------

docs/modules/path_tracking.rst

@@ -90,7 +90,7 @@ Ref:
    Conference
    Publication <http://ieeexplore.ieee.org/document/5940562/>`__
 
-.. include:: ./PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.rst
+.. include:: Model_predictive_speed_and_steering_control.rst
 
 Ref: