kalman (1).txt

\documentclass{article}
\usepackage[utf8]{inputenc}
\usepackage{graphicx}

\title{Localization}
\author{Kalman-1D }
\date{August 2020}

\begin{document}

\maketitle

\section{Introduction-What is Localization}
Robot localization is the process of determining where a mobile robot is located with respect to its environment. Localization is one of the most fundamental competencies required by an autonomous robot as the knowledge of the robot's own location is an essential precursor to making decisions about future actions.

\section{Methods Used for Localization}
\subsection{Monte Carlo localization}
\begin{abstract}
To navigate reliably in indoor environments, a mobile robot must know where it is. Thus, reliable position estimation is a key problem in mobile robotics. We believe that probabilistic approaches are among the most promising candidates to providing a comprehensive and real-time solution to the robot localization problem. However, current methods still face considerable hurdles. In particular the problems encountered are closely related to the type of representation used to represent probability densities over the robot's state space. Earlier work on Bayesian filtering with particle-based density representations opened up a new approach for mobile robot localization based on these principles. We introduce the Monte Carlo localization method, where we represent the probability density involved by maintaining a set of samples that are randomly drawn from it. By using a sampling-based representation we obtain a localization method that can represent arbitrary distributions. We show experimentally that the resulting method is able to efficiently localize a mobile robot without knowledge of its starting location. It is faster, more accurate and less memory-intensive than earlier grid-based methods.


\title{\textbf{Bayes Theorem for state Update}}
\begin{equation}
 \Pr(A|B)=\frac{\Pr(B|A)\Pr(A)}{\Pr(B|A)\Pr(A)+\Pr(B|\neg A)\Pr(\neg A)}
\end{equation}

\title{\textbf{Theorem of Total Probability }}


\begin{equation}
    \Pr(X^{t}_{i}) = \sum_{j} Pr(X^{t-1}_{j})Pr(X_{i}|X_{j})
\end{equation}


\subsection{Kalman Filter}

Kalman Filtering can be understood as a way of making sense of a noisy world. When we want to determine where a robot is located, we can rely on two things: We know how the robot moves from time to time since we command it to move in a certain way. This is called state transitioning.And we can measure the robot’s environment using its various sensors such as cameras, lidar.We do not know exactly how exactly the robot transitions from state to state since actuators are not perfect and we cannot measure the distance to objects with infinite precision. This is where Kalman Filtering comes to play.


Kalman Filtering allows us to combine the uncertainties regarding the current state of the robot (i.e. where it is located and in which direction it is looking) and the uncertainties regarding its sensor measurements and to ideally decrease the overall uncertainty of the robot. Both uncertainties are usually described by a Gaussian probability distribution, or Normal distribution. A Gaussian distribution has two parameters: mean and variance. The mean expresses, what value of the distribution has the highest probability to be true, and the variance expresses how uncertain we are regarding this mean value.

\end{abstract}

\section{1-D Kalman Filter}
\begin{center}
    \includegraphics[width=.6\textwidth]{gaussian.jpg}
\end{center}
\begin{equation}
    F(x) = e^{{{ - \left( {x - \mu } \right)^2 } \mathord{\left/ {\vphantom {{ - \left( {x - \mu } \right)^2 } {2\sigma ^2 }}} \right. \kern-\nulldelimiterspace} {2\sigma ^2 }}}
\end{equation}


Now in order to predict and update our algorithm we need to calculate new mean and new variance.
\begin{center}
    \includegraphics[width=.6\textwidth]{new_gauss.png}
\end{center}


\begin{equation}
    \hat \mu = \frac{1}{\sigma^2+r^2}[r^2\mu+\sigma^2\nu]
\end{equation}


\begin{equation}
    \hat \sigma^2 = \frac{1}{\frac{1}{r^2}+\frac{1}{\sigma^2}}
\end{equation}

\title{\textbf{Motion Update Equation}}

\begin{equation}
    \hat \mu = \mu + u
\end{equation}

\begin{equation}
    \hat \sigma^2 = \sigma^2 + r^2
\end{equation}


\title{\textbf{Problems faced throughout the implementation-}}

\begin{enumerate}
    \item The Dataset we used to implement 1-D kalman filter had its local coordinates as in the form of longitudes and latitudes.Hence it needed to be converted into cartesian coordinates  in order to predict and update the Kalman Filter algorithm.
    
    
    The formula to do so is-
    
    \begin{equation}
        X = R*cos(Lat)*cos(Lon)
    \end{equation}
    
    \begin{equation}
        Y=R*cos(Lat)*sin(Lon)
    \end{equation}
    
    \begin{equation}
       Z = R*sin(Lat)
    \end{equation}
    
    
    \item Further the values of Dataset which was used to update the equation of kalman filter did not had any position values. So to it needed to apply some basic kinematics to anticipate the values of position from its respective initial velocity and acceleration value.
    
    \item Anticipating the values of position coordinates also need the value of time.The Dataset contained the timestamp whose two consecutive difference was taken to find the dt.It also needed some prescaling as its value was too high to be used as dt.The personal prescaling used was to divide the difference by 86400(number of seconds in a day).
    

\section{Sensors Used for Localization}

\subsection{MPU6050 :-}
MPU6050 sensor module is complete 6-axis Motion Tracking Device. It combines 3-axis Gyroscope, 3-axis Accelerometer and Digital Motion Processor all in small package. Also, it has additional feature of on-chip Temperature sensor. It has I2C bus interface to communicate with the microcontrollers.


It has Auxiliary I2C bus to communicate with other sensor devices like 3-axis Magnetometer, Pressure sensor etc.


If 3-axis Magnetometer is connected to auxiliary I2C bus, then MPU6050 can provide complete 9-axis Motion Fusion output.


Let’s see MPU6050 inside sensors.


\textbf{Calculations-}


Note that gyroscope and accelerometer sensor data of MPU6050 module consists of 16-bit raw data in 2’s complement form.

 Accelerometer full scale range of +/- 2g with Sensitivity Scale Factor of 16,384 LSB(Count)/g.
 
 To get sensor raw data, we need to first perform 2’s complement on sensor data of Accelerometer and gyroscope.

After getting sensor raw data we can calculate acceleration and angular velocity by dividing sensor raw data with their sensitivity scale factor as follows-


\textbf{Accelerometer values in g-}


Acceleration along the X axis = (Accelerometer X axis raw data/16384) g.

Acceleration along the Y axis = (Accelerometer Y axis raw data/16384) g.

Acceleration along the Z axis = (Accelerometer Z axis raw data/16384) g.


\textbf{Gyroscope values in °/s }


Angular velocity along the X axis = Gyroscope X axis raw data/131 °/s.

Angular velocity along the Y axis = Gyroscope Y axis raw data/131 °/s.

Angular velocity along the Z axis = Gyroscope Z axis raw data/131 °/s.



\subsection{LIDAR-}

There are two types of LIDAR commonly used for localization:1-Dimension LIDAR and 2-Dimension LIDAR.


\textbf{1-Dimension LIDAR:-}


A 1-dimensional (1D) LIDAR sensor works much like an ultrasonic sensor - it measures the distance to the nearest object more or less along a line in front of it. 1D LIDAR sensors can often be more-reliable than ultrasonics, as they have narrower “beam profiles” and are less susceptible to interference.

1D LIDAR sensors generally output an analog voltage proportional to the measured distance, and thus connect to the roboRIO’s analog input ports.


\textbf{2-Dimension LIDAR:-}


A 2-dimensional (2D) LIDAR sensor measures distance in all directions in a plane. Generally, this is accomplished (more-or-less) by simply placing a 1D LIDAR sensor on a turntable that spins at a constant rate.

Since, by nature, 2D LIDAR sensors need to send a large amount of data back to the roboRIO, they almost always connect to one of the roboRIO’s serial buses.


\subsection{GPS sensor-}


GPS receiver uses a constellation of satellites and ground stations to calculate accurate location wherever it is located.

These GPS satellites transmit information signal over radio frequency (1.1 to 1.5 GHz) to the receiver. With the help of this received information, a ground station or GPS module can compute its position and time.


GPS receiver receives information signals from GPS satellites and calculates its distance from satellites. This is done by measuring the time required for the signal to travel from satellite to the receiver.


To determine distance, both the satellite and GPS receiver generate the same pseudocode signal at the same time.

The satellite transmits the pseudocode; which is received by the GPS receiver.

These two signals are compared and the difference between the signals is the travel time.

Now, if the receiver knows the distance from 3 or more satellites and their location (which is sent by the satellites), then it can calculate its location by using Trilateration method.

\section{Extended Kalman Filter:-}

The data we process in real life would not necessarily be Linear for all the cases. In most cases the data represents the Non-Linear cases. So in order to make sense of these Non-Linear data and to Linearize it, we use extended kalman filter.

A Kalman filter that linearizes about the current mean and covariance is referred to as an extended Kalman filter or EKF.


In something akin to a Taylor series, we can linearize the estimation around the current estimate using the partial derivatives of the process and measurement functions to compute estimates even in the face of non-linear relationships.

\textbf{Equations for Kalman Filter:-}


\textbf{Predict:}

\begin{equation}
\hat x_k = F_k  \hat x + \mu 
\end{equation}

\begin{equation}
    P_k = F_k P_{k-1}F_k^T + Q_k
\end{equation}

\textbf{Update:}

\begin{equation}
    S_k = H_kP_{k|k-1}+H^T_{k}+R_k
\end{equation}

\begin{equation}
    K_k = P_{k|k-1} + H^T_{k} + S^{-1}_{k}
\end{equation}

\begin{equation}
    \hat y = z_k - H_k\hat x_{k|k-1}
\end{equation}

\begin{equation}
    \hat x_{k|k} = \hat x_{k|k-1} + K_k\hat y_k
\end{equation}

\begin{equation}
    P_{k|k} = (I - K_kH_k)P_{k|k-1}
\end{equation}


Here $\hat x_{k|k}$ is the posterior state estimate at time K given observations up to and at time k


In the equation (12) $P_{k|k}$ is the posterior estimate covariance matrix 

Here the equation (15) is the innovation produced considering the estimate of the kalman filter

The equation (14) is the kalman gain equation.The gain matrix $K_k$ evolve independently of the measurements.Equation (12),(13),(16),(17) are used to update the kalman gain matrix.


Practical implementation of the Kalman Filter is often difficult due to the difficulty of getting a good estimate of the noise covariance matrices $Q_k$ an d $R_k$. Therefore this needs to be estimated from the sensor data.


Here the $H_k$ matrix is the Jacobian matrix which is derived from the taylor series after linearizing the data.


$${\displaystyle {{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}}}$$


$${{{\boldsymbol  {H}}_{{k}}}}=\left.{\frac  {\partial h}{\partial {\boldsymbol  {x}}}}\right\vert _{{{\hat  {{\boldsymbol  {x}}}}_{{k|k-1}}}}$$
    
    
\end{enumerate}

\end{document}