# Difference between revisions of "An Extended Kalman Filter 2010"

(→Code:) |
|||

(15 intermediate revisions by one user not shown) | |||

Line 1: | Line 1: | ||

− | ''' | + | ''' |

+ | == Introduction: == | ||

+ | ''' | ||

In the pursuit of constructing truly autonomous mobile robots, the simultaneous localization and mapping problem is considered as one of the most important problems. Simultaneous localization and mapping (SLAM) is the process by which a mobile robot can build a map of an unknown environment and at the same time use this map to compute its location. SLAM is considered as a chicken-egg problem as to identify a robots location the map is needed which the robot is still building. One classical solution to the | In the pursuit of constructing truly autonomous mobile robots, the simultaneous localization and mapping problem is considered as one of the most important problems. Simultaneous localization and mapping (SLAM) is the process by which a mobile robot can build a map of an unknown environment and at the same time use this map to compute its location. SLAM is considered as a chicken-egg problem as to identify a robots location the map is needed which the robot is still building. One classical solution to the | ||

Line 5: | Line 7: | ||

− | '''How the project works:''' | + | ''' |

+ | == How the project works: == | ||

+ | ''' | ||

Overview of the project is shown here: | Overview of the project is shown here: | ||

− | |||

− | + | [[File:Slam.png|400px]] | |

+ | Detailed process is explained below: | ||

− | '''Results:''' | + | 1. '''Observe and associate landmarks''': |

+ | |||

+ | For simplicity, I assumed true data association. That means whenever the robot sees a landmark it knows which landmark is this. Each landmark has id. Landmarks are stored in an array. If landmark with id n is not in the landmark array, it is added to the array. | ||

+ | |||

+ | 2. '''Predict next state using on odometry''': | ||

+ | |||

+ | The robots state is defined as X = [ x y theta] | ||

+ | |||

+ | Here, x = x component of robot’s position | ||

+ | |||

+ | y= y component of robot’s position | ||

+ | |||

+ | theta = robot's orientation | ||

+ | |||

+ | The robot’s initial uncertainty is defined as P, which is a 3X3 diagonal matrix. | ||

+ | The estimation of next state could be predicted using to following equation: | ||

+ | |||

+ | Next state estimation, X<sub>k+1</sub> = F<sub>k</sub> X<sub>k</sub>+ B<sub>k</sub> u<sub>k</sub> | ||

+ | |||

+ | Here, | ||

+ | F<sub>k</sub> = The Jacobian of the prediction model | ||

+ | =[1 0 -DeltaY] | ||

+ | [0 1 DeltaX] | ||

+ | [0 0 1 ] | ||

+ | DeltaY = change of y = velocity*deltaT * sin(theta) | ||

+ | DeltaX = change of x = velocity*deltaT * cos(theta) | ||

+ | B<sub>k</sub> = Input gain matrix | ||

+ | = [Cos(theta) 0] | ||

+ | [Sin(theta) 0] | ||

+ | [0 1] | ||

+ | u<sub>k</sub> = control input vector | ||

+ | = [deltaT*velocity, deltaTheta]<sup>T</sup> | ||

+ | |||

+ | Estimation of error covariance is computed using following equation: | ||

+ | Error covariance estimation, P<sub>k+1</sub> = F<sub>k</sub> P<sub>k</sub> F<sub>k</sub><sup>T</sup> + W Q W<sup>T</sup> | ||

+ | |||

+ | Here, Q = process noise = [C * (DeltaX)<sup>2</sup> 0 0] | ||

+ | [0 C * (DeltaY)<sup>2</sup> 0] | ||

+ | [0 0 C * (DeltaT)<sup>2</sup> ] | ||

+ | C = Gaussian noise | ||

+ | and W = white noise = [DeltaX DeltaY DeltaTheta]<sup>T</sup> | ||

+ | |||

+ | 3. '''Update state:''' | ||

+ | |||

+ | For each re-observed landmarks, update estimated state using re-observed landmarks using the following steps: | ||

+ | |||

+ | a) Compute innovation covariances: | ||

+ | |||

+ | Innovation covariance, S = H P<sub>k+1</sub>H<sup>T</sup> +R | ||

+ | Here, H = jacobian of the measurement model | ||

+ | = [a b 0] | ||

+ | [c d -1] | ||

+ | and a = (landmarkX – x)/range | ||

+ | b = (landmarkY – y)/range | ||

+ | c = (y -landmarkY)/range<sup>2</sup> | ||

+ | d = (x -landmarkX)/range<sup>2</sup> | ||

+ | landmarkX = x coordinate of the landmark | ||

+ | landmarkY = y coordinate of the landmark | ||

+ | range = sqrt((landmarkX – x)<sup>2</sup>+ (landmarkY– y)<sup>2</sup> | ||

+ | R = the measurement error matrix | ||

+ | = [rangeError 0 ] | ||

+ | [0 bearingError] | ||

+ | |||

+ | We also need another matrix D which is landmark's portion of final measurement model, use of D will be explained later. | ||

+ | D = [-a -b] | ||

+ | [-c -d] | ||

+ | |||

+ | b) Compute kalman gain: | ||

+ | Kalman gain, K = P<sub>k+1</sub>H<sup>T </sup>* S<sup>-1</sup> | ||

+ | |||

+ | c) Update estimation X<sub>k+1</sub> based on observation z | ||

+ | |||

+ | Updated state, X = X<sub>k+1</sub>+ K(z – HX<sub>k+1</sub>) | ||

+ | |||

+ | Update P<sub>k+1</sub>, | ||

+ | Updated error covariance, P = (I – KH)P<sub>k+1</sub> | ||

+ | Where I is the identity matrix. | ||

+ | |||

+ | ''' | ||

+ | |||

+ | ==Java library for SLAM == | ||

+ | ''' | ||

+ | I used kalman filter java library, javaslam[[http://ai.stanford.edu/~paskin/slam/tjtf-java.zip]]. This library provides functions for motion update and measurement update. Steps for using javaslam are explained below: | ||

+ | |||

+ | 1. Create an instance of KalmanSLAMFilter: | ||

+ | |||

+ | KalmanSLAMFilter kf = new KalmanSLAMFilter(X,P); | ||

+ | Where X = robot’s initial state | ||

+ | P = robot’s initial error covariance | ||

+ | |||

+ | 2. To predict next state estimate, call the motion function: | ||

+ | |||

+ | kf.motion(Bk.times(uk).getColumnPackedCopy(),Fk, WQW<sup>T</sup>); | ||

+ | |||

+ | here, Bk, uk, Fk and WQW<sup>T</sup> are explained in step 2 of the detailed description. | ||

+ | |||

+ | 3. To update state estimate using re-observed landmarks, call the measurement function: | ||

+ | |||

+ | kf.measurement(id, new double[] {0,0}, H,D, R, z); | ||

+ | here, id = landmark id | ||

+ | z = [observedRange, observedBearing] | ||

+ | |||

+ | and H,D,R,z are explained in step 3 of the detailed description. | ||

+ | |||

+ | |||

+ | ''' | ||

+ | |||

+ | == Results: == | ||

+ | ''' | ||

This implementation is evaluated with the following assumptions: | This implementation is evaluated with the following assumptions: | ||

+ | |||

–Whatever the robot sees is a landmark | –Whatever the robot sees is a landmark | ||

+ | |||

–Data association problem is not considered | –Data association problem is not considered | ||

+ | |||

–Robot odometry data and laser data are simulated | –Robot odometry data and laser data are simulated | ||

− | Evaluation is done using | + | |

+ | Evaluation is done using 8 landmarks and straight line path. | ||

Result is shown in figure below. | Result is shown in figure below. | ||

− | [ | + | |

+ | [[File:Slam_result.png]] | ||

In the picture, the red line is the ground truth, green line shows the odometry data and the blue line shows the robot path estimated by the kalman filter. | In the picture, the red line is the ground truth, green line shows the odometry data and the blue line shows the robot path estimated by the kalman filter. | ||

− | |||

− | |||

− | Code can be downloaded from | + | This implementation did not converge to the ground truth when the path is circular. |

+ | |||

+ | == Conclusion: == | ||

+ | EKF Slam works well when the following situation holds: | ||

+ | |||

+ | 1. Initial estimations of the robot state and error covariance are almost correct. If difference between actual robot position and initial estimation is high, EKF does not converge. | ||

+ | |||

+ | 2. Error model and robot motion model are almost linear. EKF-SLAM does not converge where large angular error is present. | ||

+ | |||

+ | 3. Data association is almost perfect. | ||

+ | |||

+ | In most real world situation, these assumption do not hold. More robust methods are needed in those cases. | ||

+ | |||

+ | == Code: == | ||

+ | |||

+ | Code can be downloaded from [https://docs.google.com/leaf?id=0ByOY6vHGMB8PZjk1ZDJlODQtMWZiYS00NDI1LWEwNmMtMDUwNmJmZmY3YzJm&hl=en here] | ||

+ | |||

+ | To run the code, run SlamSimulation.java. | ||

+ | |||

+ | == Useful Links: == | ||

+ | |||

+ | 1. [http://bilgin.esme.org/BitsBytes/KalmanFilterforDummies/tabid/196/Default.aspx Kalman filter for dummies] | ||

+ | |||

+ | 2.[http://ocw.mit.edu/NR/rdonlyres/Aeronautics-and-Astronautics/16-412JSpring-2005/9D8DB59F-24EC-4B75-BA7A-F0916BAB2440/0/1aslam_blas_repo.pdf SLAM for dummies ] | ||

+ | |||

+ | 3. [http://www.ri.cmu.edu/pub_files/pub4/kurth_derek_2004_1/kurth_derek_2004_1.pdf Range-Only Robot Localization and SLAM with Radio] | ||

+ | |||

+ | 4.[http://ai.stanford.edu/~paskin/slam/ JavaSlam] |

## Latest revision as of 20:29, 23 March 2010

## Contents |

## Introduction:

In the pursuit of constructing truly autonomous mobile robots, the simultaneous localization and mapping problem is considered as one of the most important problems. Simultaneous localization and mapping (SLAM) is the process by which a mobile robot can build a map of an unknown environment and at the same time use this map to compute its location. SLAM is considered as a chicken-egg problem as to identify a robots location the map is needed which the robot is still building. One classical solution to the SLAM problem is the EKF-SLAM method. EKF-SLAM uses Kalman filter, which is an efficient recursive filter that estimates the internal state of a linear dynamic system from a series of noisy measurements. Goal of this project is to implement and simulate the EKF-SLAM method for solving the SLAM problem.

## How the project works:

Overview of the project is shown here:

Detailed process is explained below:

1. **Observe and associate landmarks**:

For simplicity, I assumed true data association. That means whenever the robot sees a landmark it knows which landmark is this. Each landmark has id. Landmarks are stored in an array. If landmark with id n is not in the landmark array, it is added to the array.

2. **Predict next state using on odometry**:

The robots state is defined as X = [ x y theta]

Here, x = x component of robot’s position

y= y component of robot’s position

theta = robot's orientation

The robot’s initial uncertainty is defined as P, which is a 3X3 diagonal matrix. The estimation of next state could be predicted using to following equation:

Next state estimation, X_{k+1}= F_{k}X_{k}+ B_{k}u_{k}Here, F_{k}= The Jacobian of the prediction model =[1 0 -DeltaY] [0 1 DeltaX] [0 0 1 ] DeltaY = change of y = velocity*deltaT * sin(theta) DeltaX = change of x = velocity*deltaT * cos(theta) B_{k}= Input gain matrix = [Cos(theta) 0] [Sin(theta) 0] [0 1] u_{k}= control input vector = [deltaT*velocity, deltaTheta]^{T}

Estimation of error covariance is computed using following equation:

Error covariance estimation, P_{k+1}= F_{k}P_{k}F_{k}^{T}+ W Q W^{T}Here, Q = process noise = [C * (DeltaX)^{2}0 0] [0 C * (DeltaY)^{2}0] [0 0 C * (DeltaT)^{2}] C = Gaussian noise and W = white noise = [DeltaX DeltaY DeltaTheta]^{T}

3. **Update state:**

For each re-observed landmarks, update estimated state using re-observed landmarks using the following steps:

a) Compute innovation covariances:

Innovation covariance, S = H P_{k+1}H^{T}+R Here, H = jacobian of the measurement model = [a b 0] [c d -1] and a = (landmarkX – x)/range b = (landmarkY – y)/range c = (y -landmarkY)/range^{2}d = (x -landmarkX)/range^{2}landmarkX = x coordinate of the landmark landmarkY = y coordinate of the landmark range = sqrt((landmarkX – x)^{2}+ (landmarkY– y)^{2}R = the measurement error matrix = [rangeError 0 ] [0 bearingError]

We also need another matrix D which is landmark's portion of final measurement model, use of D will be explained later.

D = [-a -b] [-c -d]

b) Compute kalman gain:

Kalman gain, K = P_{k+1}H^{T }* S^{-1}

c) Update estimation X_{k+1} based on observation z

Updated state, X = X_{k+1}+ K(z – HX_{k+1})

Update P_{k+1},

Updated error covariance, P = (I – KH)P_{k+1}Where I is the identity matrix.

## Java library for SLAM

I used kalman filter java library, javaslam[[1]]. This library provides functions for motion update and measurement update. Steps for using javaslam are explained below:

1. Create an instance of KalmanSLAMFilter:

KalmanSLAMFilter kf = new KalmanSLAMFilter(X,P); Where X = robot’s initial state P = robot’s initial error covariance

2. To predict next state estimate, call the motion function:

kf.motion(Bk.times(uk).getColumnPackedCopy(),Fk, WQW^{T});

here, Bk, uk, Fk and WQW^{T} are explained in step 2 of the detailed description.

3. To update state estimate using re-observed landmarks, call the measurement function:

kf.measurement(id, new double[] {0,0}, H,D, R, z); here, id = landmark id z = [observedRange, observedBearing]

and H,D,R,z are explained in step 3 of the detailed description.

## Results:

This implementation is evaluated with the following assumptions:

–Whatever the robot sees is a landmark

–Data association problem is not considered

–Robot odometry data and laser data are simulated

Evaluation is done using 8 landmarks and straight line path.
Result is shown in figure below.

In the picture, the red line is the ground truth, green line shows the odometry data and the blue line shows the robot path estimated by the kalman filter.

This implementation did not converge to the ground truth when the path is circular.

## Conclusion:

EKF Slam works well when the following situation holds:

1. Initial estimations of the robot state and error covariance are almost correct. If difference between actual robot position and initial estimation is high, EKF does not converge.

2. Error model and robot motion model are almost linear. EKF-SLAM does not converge where large angular error is present.

3. Data association is almost perfect.

In most real world situation, these assumption do not hold. More robust methods are needed in those cases.

## Code:

Code can be downloaded from here

To run the code, run SlamSimulation.java.

## Useful Links:

3. Range-Only Robot Localization and SLAM with Radio

4.JavaSlam