karanchawla/gps_imu_kalman_filter

Fusing GPS, IMU and Encoder sensors for accurate state estimation.

ExtendedKalmanFilter

EKF to fuse GPS, IMU and encoder readings to estimate the pose of a ground robot in the navigation frame.

Wikipedia writes: In the extended Kalman filter, the state transition and observation models need not be linear functions of the state but may instead be differentiable functions.

x_k = g(x_k), u_k-1 + w_k-1
z_k = h(x_k) + v_k

Where w_k and v_k are the process and observation noises which are both assumed to be zero mean Multivariate Gaussian noises with covariance matrix Q and R respectively.

The function g can be used to compute the predicted state from the previous estimate and similarly the function h can be used to compute the predicted measurement from the predicted state. However, g and h cannot be applied to the covariance directly. Instead a matrix of partial derivatives (the Jacobian matrix) is computed.

At each time step, the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations. This process essentially linearizes the non-linear function around the current estimate.

Here we have a velocity sensor (encoders/GPS velocity), which measures the vehicle speed (v) in heading direction (psi), a yaw rate sensor (psi_dot) and an accelerometer which measures longitudinal velocity which both have to fused with the position (x & y) from the GPS sensor.

References

1. @balzer82 for his tutorials on Kalman Filters.
2. Probabilistic Robotics by Thrun, Burgard, and Fox.

gps_imu_kalman_filter's Issues

No example data Data.csv

Can you share one example data as descripted in main.cpp? Thanks a lot.

jacobian calculate error

In calculate_jacobian function, r13 should be turn_radius * ( cos(dt*psi_dot + psi) - cos(psi) ).

GPS datas

I runned the filter with several data sets and find out that the GPS state is totaly out of state.
The acceleration and velocity are correct but not the GPS and i don't succed in finding out what's the problem...
Pls help me =(
I'll link a picture of what's happening (red is the datas and blue the state).

Can you give me some instructions about your state variables and state transfer function?

Thank you for sharing ! I have a question about your ekf algorithm. I don't know what state variables are you choose and how to get the state transfer equation and measurement equation ? Can you give me some advice and papers about your algorithm ?

After some hard work, I got the wrong answer

I'm glad to see your sharing. After I wrote the main function call myself, when I ran with the dataset, I found that the result was totally wrong (compared with Python's result). Is this finished product, or is it partially incomplete? I was confused.

Finally, the output state is found as follows.

another question ,I've noticed your approach to Kalman time DT, which I'm confused about.

Anyway, thank you a lot！

The question of head file "csv_reader.hpp" and member function "void EKF::updateJA(const double dt)" in the class of ekf

when I read the code, I find that there are lack of a head file "csv_reader.hpp".Can you update it and make this project integrated.
There are also lack of a definition of "calculate_jacobian()" in the member function of "void EKF::updateJA(const double dt)" in ekf.cpp.
Sir, can you help me fix these problems. thank you very much.

example data

Thank you for sharing.
Can you upload some example data or give a description of the input data format?
Urgently need your reply.
Thank you again.

papers

hello，
Is there any papers about your work？ If yes, can you give me a URL or paper name?

Thkan you.

what's the ground truth?

do you know public dataset that can prove my algorithm works

help needed to use this package

Dear Karanchawla, thank you for sharing useful code. Currently, I am looking to fuse the GPS and IMU data for localization self-driving cars. Your fusion approach looks useful for my needs. But am unable to understand, how to make use of this package without any test case. As i checked in the main loop, there is no function to read the data to start the fusion process.

Could you please, give little more detail about how to start the fusion process? really It will be more helpful to further improve this.

regards,
Ajay

read_imu_data and read_gps_data function not being called?

I would have expected the read_imu_data and the read_gps_data functions to be called in the loop . May be in the function loop in run_fusion.cpp but I dont see it being called anywhere. How is the data being read from the sensors to get measurements across time intervals.
or am I missing something?
Do you expect all the readings to be available before hand or can this also work in real time ?

Yes, I will upload it once I get home tonight.

Yes, I will upload it once I get home tonight.

Originally posted by @karanchawla in #2 (comment)

where is the calculate_jacobian function

where is the calculate_jacobian function ?

How to compile and use this program

Hello，how can I compile this project and use it on my computer？

Main loop for the filter

As a function is written as:

void GpsIns::loop()
{
//m.lock();
std::lock_guardstd::mutex lock(m);
set_data();
filter->process(*_sensor_data);
_prev_gps_counter = _gpscounter;
//m.unlock();
}

The function above is being introduced as "Main loop for the filter". I am confused about the functions:read_gps_data, read_imu_data, read_encoders seem not working here ,and some counters are forgot to plus "1". Is there some work should be done by ourselves and you simplify the main loop here? Thanks, just too confused.

can you give me some details about the main.c？ GpsIns have not beed called?