ME465 Lab 2
Introduction
Mobile robots operating outdoors can use GPS for localization. While GPS is very good, there is noise in its measurements. A Kalman Filter can be used to filter out some of this noise by utilizing knowledge about how the robot moves.
Robot Class
In your code, use the Robot.m MATLAB class.
Initialize the class using,
robot = Robot(pause_duration);
where pause_duration is the time to wait before plotting the next point.
An updated measurement is received from the GPS at a rate of 5 Hz.
These measurements are returned by the function,
measurement = robot.get_measurement();
where measurement is a measurement of the current robot position from the GPS.
This is a 2 element column vector consisting of the number of meters to the east of an origin and the number of meters north of the origin.
This measurement can be considered to have noise,
The function,
input = robot.get_input();
provides the current input to the robot.
It is a 2 element column vector of the force applied to the robot in Newtons to move it to the east and the north respectively.
This function provides many non-zero inputs for the robot, but after the internally stored data is exhausted, the function will return a zero vector.
At such a time, your code should consider having your script exit.
To allow you to visualize the function on the Kalman Filter, the function,
robot.plot(estimate, variance)
is provided.
The arguments for this function are first, estimate, the 4 element column vector of the current estimate of the robot location, and variance the 4 by 4 current variance matrix of the estimate from the Kalman Filter.
These two argument assume a state vector,
These arguments are only used for plotting.
Robot Description
The motion of the robot is defined by the equations,
\[\begin{aligned}\dot{v}_x&=\frac{F_x}{m}+\nu\quad\text{and}\\ \dot{v}_y&=\frac{F_y}{m}+\nu,\end{aligned}\]where $m=9\ \text{kg}$, $F_x$ and $F_y$ are the forces applied to move the robot in the $x$ and $y$ axes, $v_x$ and $v_y$ are the velocities of the robot motion in the $x$ and $y$ axes, and $\nu\sim\mathcal{N}(0, 1\times10^{-3})$. Similarly, the equations,
\[\begin{aligned}\dot{x}_x&=v_x+\upsilon\quad\text{and}\\ \dot{x}_y&=v_y+\upsilon,\end{aligned}\]convert the velocity to a position where $x_x$ and $x_y$ are the position of the robot in the $x$ and $y$ axes, and $\upsilon\sim\mathcal{N}(0, 5\times10^{-4})$. These equations can be encoded in a state space model, then discretized.
Initial Conditions
You may assume that the robot starts at an initial location $(x_x,\,x_y)=(50,\,-30)$, and with zero velocity. These initial conditions have a variance of $9\ \text{m}^2$ for the robot position, and $0.2\ \text{m}^2/\text{s}^2$ for the velocity. Using a Kalman Filter, estimate the path of the robot from the GPS measurements and the inputs to the robot.
Checklist
To complete this lab, the following must be accomplished using the instructions above:
- Determine the continuous time state space model of the robot.
- Discretize the continuous time state space model to create a discrete time model of the robot
- Initialize the robot class.
- Set initial values for the robot state estimate and variance.
- Get a measurement and input from the robot class.
- Plot the current state estimate and variance.
- Create a loop to iteratively get measurements and inputs, then plot.
- Have the loop exit when a zero vector input is received.
- Implement the Kalman Filter code inside the loop to filter the incoming measurements.
Lab Report
In your report, along with the normally required information, discuss the benefits of using the Kalman Filter to localize the robot.