Kalman Filter

The Kalman Filter provides a way to filter GPS data, reducing the effects of noise and increasing accuracy.

The Kalman Filter Equation

The problem with GPS data

The idea behind the bike’s GPS system is to receive live GPS data while the bike is running, and use that data to determine the current position of the bike and where it needs to go. In theory, simply receiving GPS data and interpreting it without any filtering would work properly, however, with inaccuracies and noise affecting the GPS, a filter is needed to more accurately determine the location of the bike. The Kalman filter is the bike’s current solution to sensor fusion and filtering method for GPS data, and it helps reduce noise and increase the accuracy of the GPS plotting.

Sensor Fusion

As previously mentioned, the sole usage of GPS data to determine position is theoretically fine, but impractical. Specifically, the motivating factor for sensor fusion is a path width of approximately 2 meters, while the current GPS has a published accuracy of 2.5m. The purpose of sensor fusion is to fuse multiple streams of information about the bike state together, as a way of obtaining more accurate position estimation and improving the localization algorithm. The bicycle collects data about its lean angle, steer angle, velocity, and current x- and y- position estimates, along with the position updates from the GPS.

In previous bike tests, we realized that the gyroscope and the accelerometer would still record a change, even if the bicycle was still. Most of the localization algorithm relies more on the signals and data received from the IMU to determine its relative location. Especially in straight travel, we can use the rear wheel to increment the bicycle's internal x- and y- coordinates for travel.

Our current solution for sensor fusion is to use a Kalman-like model. The information received from the GPS coordinates, IMU signals, and Hall sensors are fused to augment the accuracy of our location estimates. This model for sensor fusion only relies on the previous state and the current observations to estimate where the current location is. Throughout the semesters, the sensor fusion algorithm has been modified as more navigation tests fail.

Basic Filtering

The general idea behind the Kalman filter is to combine a number of averaging techniques that give an accurate estimate of the actual location. One of these techniques used is Dead Reckoning, which determines position based on previous position and speed. Say the position of the bike at time k is represented by the variable x_k. We can inductively define x_k to be equal to ax_k-1, where a is some constant, and x_k-1 is the position of the bike at time k-1. Using this, we define each position based on the previous position, only requiring GPS data for the start position. In the case of the bike, the position needs to be calculated as pairs of x y or lat long coordinates, and the function would be applied to each separately, making the position formula (x_k,y_k) = (a+x_k-1, b+y_k-1). Notice that a and b are being added to the old position rather than multiplied. This is because we can calculate the distance traveled during the timestep between kand k-1using the bike’s velocity data and the heading at time k-1. We use basic trig functions to calculate this distance, with a=speed cos(heading) timestep and b=speed sin(heading) timestep.

Another simple formula used in the Kalman filter states that the observed position at time k is equal to the actual position plus some random noise value. Although intuitive, this formula is an integral part of the filter, as we can combine it with the first formula to get the equation x_k = x_k-1+g_k(z_k-x_k-1). In this equation, x_k represents our estimated state at time k, g_k is our gain at time k and z_k is our observed state at time k. This equation estimates the state taking into account both the observed state and the previous state, and the gain is some value between 0 and 1 that represents which one you put more emphasis on. We can calculate gain given the average noise of the GPS, for example, if the gain was 1, x_k would be equal to z_k, in other words, we have no noise. A more in depth explanation of the Kalman Filter equation can be found here.

Some of the issues with the Kalman filter are that it depends a lot on noise, otherwise, it could be inaccurate at times. Additionally, it assumes Gaussian distribution of noise, lack of which could cause more inaccuracies.

Localization with the Pi

https://docs.swiftnav.com/wiki/ROS_Integration_Guide contains information about the connection between ROS, Swift, and other sources of information, such as the IMU.

Swift publishes two messages, sensor_msgs::NavSatFix, which produces SPP data, and nav_msgs::Odometry, which has geometry_msgs PoseWithCovariance and geometry_msgs TwistWithCovariance, that must be defined. We are currently unable to access SPP, or single point position, data, and are trying to understand why. PoseWithCovariance and TwistWithCovariance contain the pose and a covariance matrix with 36 entries (http://docs.ros.org/melodic/api/geometry_msgs/html/msg/PoseWithCovariance.html).

After downloading the robot_localization package to the Pi, there are template files that must be adjusted to fuse sensors and produce position. Modify the launch file for the required settings for the bike. The "Fusing GPS+IMU+Odometry" section is useful for determining what changes to make to the launch file. It also provides a diagram explaining the connections between the different sensors. Set "two_d_mode" to true since for now, we only care about the 2D environment for the bike. This may change when we add steepness calculations.

All files are under robot_localization/launch, and the file containing this information is sensorFusion. This is a navsat_transform that takes in three inputs, the nav_msgs::Odometry, msgs::NavSatFix, and sensor_msgs/Imu and uses the Kalman Filter to produce the position. The covariance matrix was largely kept the same, we added an error, but kept the rest 0. We need to test with this matrix and then change if necessary. The robot_pose_ekf can be used to calculate the pose based on the position and the odometry, https://github.com/ros-planning/robot_pose_ekf. It uses the relative pose differences between different sensors with the ekf to produce a new pose.

Examples of defining pose and twist are http://wiki.ros.org/turtlesim/Tutorials/Go%20to%20Goal and https://www.programcreek.com/python/example/88501/geometry_msgs.msg.Pose, http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom. After creating a pose publisher, define the x,y,z coordinates for the pose and the quaternion, or orientation coordinates x,y,z, and w. There is also function, createQuaternionMsgFromYaw(th), that converts yaw to quaternion. Then, define the covariance with an array.

Bike Code

The code for the bike’s Kalman Filter is all stored under navigation, within the file kalman_node.py and the files under localization.

kalman_node.py

• Responsible for ROS communication and receiving data from sensors.
• Bike_state_listener receives steer, yaw, speed, and timestep.
• Gps_listener receives latitude and longitude. Note that these lat and longs values are defined globally, and must be redefined using a fixed origin.
• Reset_listener resets the state to receive new sensor info.
• Main_loop loops through states.

localization/kalman.py

• Takes in raw data and outputs filtered data.
• Stores and updates two Kalman states, k1 and k2.
• update updates the state whenever there are sensor observations, uses Kalman Filter equations to compute gain and predict new states.
• kalman_retro_old filtered data retroactively, taking in a matrix containing raw data and outputting a matrix of filtered data.
• kalman_real_time filters data in real time, taking in the previous Kalman state and previously observed state and applying filtering equations, returning a new Kalman state and observed state.

localization/kalman_driver.py

• Given CSV files, graphs GPS data and Kalman data side by side.

localization/sensor_fusion.py

• Contains the sensor fusion equation for Kalman filtering.

nav/expensiveCompass.py

• New File (2020) that publishes the heading in nav_instr
• Also adjusts for the tilt in the bike to produce heading
• Created a new launch file, expensiveGPS.Launch, that uses this file

robot_localization/SensorFusion.Launch

• New file (2020), package downloaded from https://github.com/cra-ros-pkg/robot_localization/tree/melodic-devel/launch
• Contains information for robot localization based on information presented earlier, in the Localization with the Pi section