Ackermann EKF Performance

[rowandempster]

@muhtasim001 Originally reported on Discord that he is looking to diagnose issues and/or improve the performance of the Ackermann EKF they have implemented.

Derivation:

Results:

[rowandempster]

@muhtasim001 In the result images, what is the source of the "Odometry" timeseries? Is that signal being used as the measurement in the EKF derivation?

[rowandempster]

Also, can you provide the following to reproduce your results:

1. A rosbag (mcap) of the data used to produce the results you plotted
2. The Foxglove layout .json file used to plot the results
3. Instructions for how to run the EKF node in an isolated docker environment

[rowandempster]

This is the node being compiled right: https://github.com/WATonomous/wato_f1tenth/blob/mapping-and-localization/src/robot/ackermann_ekf/include/ekf_node.hpp

This code isn't being used for anything correct: https://github.com/WATonomous/wato_f1tenth/blob/mapping-and-localization/src/robot/ackermann_ekf/include/ekf_node.hpp

[rowandempster]

wato_f1tenth/src/robot/ackermann_ekf/src/ekf_node.cpp

Lines 31 to 45 in f33e222

	void EKF_NODE::imuCallBack (sensor_msgs::msg::Imu::SharedPtr msg) {
	current_imu = *msg;
	}
	/**
	* @brief
	* stores the most recent steering message for EKF
	*/
	void EKF_NODE::steeringCallBack(std_msgs::msg::Float32::SharedPtr msg) {
	current_steering = *msg;
	}
	void EKF_NODE::throtelCallBack(std_msgs::msg::Float32::SharedPtr msg) {
	current_throtel = *msg;
	}

In the callback of the process inputs you cache the inputs which are then used when we get an odometry message:

https://github.com/WATonomous/wato_f1tenth/blob/f33e2228d3545898e456c6bcd84f28192ba46245/src/robot/ackermann_ekf/src/ekf_node.cpp#L91C13-L91C21

Since IMUs (and steering / throttle readings) usually produce data at a rate many times higher than the odometry message rate, this means that most of the IMU, steering, and throttle data is not being used.

The pattern that is usually followed by EKF implementations is to have the process inputs callback(s) drive the EKF forward via the prediction step: https://en.wikipedia.org/wiki/Extended_Kalman_filter#Predict

[rowandempster]

wato_f1tenth/src/robot/ackermann_ekf/include/ekf_node.hpp

Lines 64 to 65 in f33e222

	vec7d *mu;
	matrix7d *sigma_t;

Why do these eigen matrices have to be pointers?

[rowandempster]

wato_f1tenth/src/robot/ackermann_ekf/src/ekf_node.cpp

Lines 170 to 175 in f33e222

	vec7d Z = EKF_NODE::observationCreator(odom,current_imu);
	matrix7d G = EKF_NODE::calculateJacobianG(mu_p,current_steering);
	//predict step
	vec7d mu_bar = EKF_NODE::modelUpdate(mu_p,current_steering);
	matrix7d sigma_t_bar = G * sigma_t_p * G.transpose() + R;

The ordering of operations is off here. Currently you are computing the expected observation using the state at t-1. What you want to do is propagate the state forward using all the process input messages that came in before the odometry message, and then compute the expected observation based on that state.

[rowandempster]

wato_f1tenth/src/robot/ackermann_ekf/src/ekf_node.cpp

Line 170 in f33e222

vec7d Z = EKF_NODE::observationCreator(odom,current_imu);

I'm confused by this line, why do you need the IMU data to create an observation? The IMU data should be driving the "Predict" part of your EKF, not have anything to do with the measurement / update step - unless this is an EKF derivation I'm not familiar with.

[rowandempster]

I can go through your derivation of the state transition jacobian and implementation of the predict and update matrices, but before I do I think there are some more fundamental concepts of what EKFs are used for and what performance is expected that we should go over.

Stuff to clear up before diving into to any math / code:

• Where is the measurement coming from, what reference is the EKF supposed to be converging to in the long term?
• What is driving the process equation? IMU or steering information, or both?

[muhtasim001]

if you now look in the mapping and localization branch, i have created a new launch file in bringup_robot along with added bag files can be used to produce the results seen on the graphs I have sent.

the following is assumed :

• you are in a Linux environment (I am so sorry I don't know how to use windows T_T that well for ros, ping Rodney on discord, he gets how to run it windows better)
• you have an NVIDIA GPU with working drivers in Linux with the NVIDIA container toolkit installed (if not, you just need to make a few modification to the robot docker compose file, just comment out the runtime and the NVIDIA GPU mount)
• you are using VsCode and necessary extension to attach VsCode to the container

the following are instruction on how to run them in a docker container:

1. clone the wato f1teenth git repo
2. switch to the mapping-and-localization remote branch using git checkout -b mapping-and-localization origin/mapping-and-localization if you want a more minima container , you can also use the ekf_testing branch, that only has the minimum necessary files
3. after switch, run ./watod build (to build the container) and once its built run the with ./watod up
4. attach the the robot_dev container and go the the /home/bolty/amnet_ws
5. run colcon build
6. use the ekf_testing.json for the foxglove layout
7. run ros2 launch bringup_robot ekf_investigation

this will run the run the bag files along with launching the node and shut every thing down once the recording has finished
the there are 2 bag files one is steady and bassicaly at constant low-medium speed, while the fast one can be describe as a 'racing or qualifying' lap.

[muhtasim001]

the ekf8d does not get complied, that is a remnant form an idea i wanted to peruse but didn't

[muhtasim001]

I honestly don't know everything about implementing an ekf, so most of my work is me following this article :
https://soulhackerslabs.com/sensor-fusion-with-the-extended-kalman-filter-in-ros-2-d33dbab1829d
my honest performance goals for the filter could be in the same neighborhood as the 8d example from the article

[muhtasim001]

the mu and sigma_t are pointers because the ekf would collapse / the values would become Nan when I don't make them pointers and constant perform Nan checks on them.