ai-imu-dr icon indicating copy to clipboard operation
ai-imu-dr copied to clipboard

Published 20 hours ago • verified publisher icon mbrossar

Implementation For RC Scale Car

Open colem404 opened this issue 4 years ago • 9 comments

Hello. I recently read your paper and I am interested in implementing your dead reckoning algorithm on an RC car equipped with a raspberry pi, GPS, and IMU. I was wondering if you had any suggestions for where to start if I wanted to modify the code to work with data I record from the car to train the neural network and then test the algorithm in real time. From glancing through the code, my intuition is that I need to change the datasets.py class and it would be nearly plug and play, but I thought that I would get your insight first.

colem404 avatar Feb 24 '20 05:02 colem404

Hello mbrossar,

I find this repository very interesting and thank you for making this open source. I tested this with the KITTI test dataset as guided in the page and I got perfectly same set of results as expected in this page. Great set of instructions!! Thanks

Now I am trying to implement this real time for the turtle-bot which has GPS and IMU. when I had the glance at the code, it is not straight forward to implement with own data. As mentioned above in the post Please can you suggest where to begin with and some suggestions would be really grateful.

Best Regards, Harish

sharish33 avatar Mar 12 '20 09:03 sharish33

Hello Sharish,

I recognize that invariant Kalman theory is not straightforward. My best suggestion for reading are:

A. Barrau and S. Bonnabel, “Invariant Kalman Filtering,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 1, no. 1, pp. 237–257, 2018.

A. Barrau and S. Bonnabel, “The Invariant Extended Kalman Filter as a Stable Observer,” Transaction on Automatic Control, vol. 62, no. 4, pp. 1797–1812, 2017.

An example of well application is R. Hartley, M. G. Jadidi, J. W. Grizzle, and R. M. Eustice, “Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation,” The International Journal of Robotics Research, 2020.

What you need to understand is that the error is NOT e = x - \hat{x} but defined in equations (22) to (25).

I think the question you search to answer is: What is the Jacobian for GPS measurement y = p ?

To obtain it, I start by linearizing the error (22) for rotation and position (we do not need velocity here)

\exp(\xi^R) \simeq Id + (\xi^R)_\times = R \hat{R}^T \xi^p = p - R \hat{R}^T \hat{p} = p - (Id + (xi^R)_\times) \hat{p}

\xi^p = p - \hat{p} - (\xi^R)_\times \hat{p} (*)

Now, we write the residual of the measurement

y - \hat{y} = p - \hat{p}

From (*), we have

y - \hat{y} = \xi^p + (\xi^R)_\times \hat{p}

As (a)_x b = (b)_x a, we finally obtain

y - \hat{y} = (-\hat{p})_\times \xi^R + \xi^p

The Jacobian is thus Id_3 for the part of \xi^p as in a stantard Kalman filter, plus a new part of the Jacobian (-\hat{p})_\times for orientation, i.e.

H = [ (-\hat{p})_\times ~~ 0 ~~Id_3 ~~0 ~~... ]

Do you need more clarifications ?

Best regards,

Martin

mbrossar avatar Mar 12 '20 14:03 mbrossar

Hi Martin,

Thank you very much for your response on my query. I will look into those papers for better understanding. May be I misleaded you with my question. I have a ros bag file recorded from real time turtlebot having IMU data. Now I am planning to apply this algorithm to my bag file and willing to get the results how the algorithm behaves to my real data.Please can you give some suggestions for where to start if I wanted to modify the code to work with my bag file I have, to train the neural network and then test the algorithm and to get the results as like with KITTI test data.

Best Regards, Harish

sharish33 avatar Mar 12 '20 15:03 sharish33

Hi,

I can suggest  you to start as simple as possible. If it is only a rosbag, you can manually correct for imu biaises and imu misalignment. You thus have only a filter with a state space of size 9. Then, without training anything, tune the filter parameters to obtain good initial guess. And then you can start training, adding biaises,...

Best regards,

Martin

Le 12 mars 2020 à 16:20, à 16:20, sharish33 [email protected] a écrit:

Hi Martin,

Thank you very much for your response on my query. I will look into those papers for better understanding. May be I misleaded you with my question. I have a ros bag file recorded from real time turtlebot having IMU data. Now I am planning to apply this algorithm to my bag file and willing to get the results how the algorithm behaves to my real data.Please can you give some suggestions for where to start if I wanted to modify the code to work with my bag file I have, to train the neural network and then test the algorithm and to get the results as like with KITTI test data.

Best Regards, Harish

-- You are receiving this because you commented. Reply to this email directly or view it on GitHub: https://github.com/mbrossar/ai-imu-dr/issues/41#issuecomment-598247205

mbrossar avatar Mar 12 '20 18:03 mbrossar

Hi Martin,

Thank you for your inputs. I am trying to implement the algorithm with real time imu data. I will let here posted with further results in my implementation.

Thank you for your support !!

Best Regards, Harish

sharish33 avatar Mar 13 '20 08:03 sharish33

Hi Martin,

I started reading the code and understanding the algorithm. But with the existing algorithm I am bit lost to implement for real time IMU data set. please could you elaborate the steps mentioned above to implement for real time data, that would be great and helpful.

Also please let know if you have tested the algorithm for real time data and this is feasible to implement.

Best Regards, Harish

sharish33 avatar Mar 18 '20 08:03 sharish33

Hi Sagi,

Could you be please describe precisely where you meet difficulties ? Is it for understanding the filter, implementing it in e.g. ros, ... ? If the problem concerns just real-time implementation, you have just to call the filter each time an IMU message happens and performs both propagation and update. The only difficulty is regarding the network, that has to conserve in memory a windows of past measurements to infer covariance (to compute covariance at time n, measurements at times n-N,...,n-1 are needed). I have tested for real-time, it is feasible.

All the best,

Martin

mbrossar avatar Mar 18 '20 15:03 mbrossar

Hi Martin,

Thanks for your replies. At least for me, the training of the neural network is the difficult part. It seems to rely on your dataset class derived from KITTI. However, I have a csv file where I have saved the data from a gps (for ground truth) and IMU. So the question is how should I format my data to work with train_torch_filter.py or make a compatible dataset.py? I think this may also be what Harish was asking.

On another note, I am also not completely sure I understand the loss function. Could you explain it?

Thanks again, Demetris

colem404 avatar Mar 18 '20 16:03 colem404

Hi Martin,

Thank you very much for your time, help and inputs here in the implementation. Sorry for the late response from my side, I took a little off from my work. Coming to implementation of algorithm to real time data, below are my questions, some of my questions may be dumb, please correct me if I am wrong and missing something :

  1. Really I studied hard to understand the IEKF filter. Hope now I understood to implement the IEKF. In your previous comment mentioned about to call the filter when it receives the IMU message, here calling the filter means to call the IEKF or the whole AI filter algorithm(main.py)?
  2. Ros bag file has IMU and GPS. How could i use the IMU/GPS sensor input because I am little confused with following pairs described in code. please can you detail about difference of these with IMU data. (ax, ay, az) (af, al, au) (wx, wy, wz) (wf, wl, wu) (vn, ve, vf) (vf, vl, vu)

When comes to neural network,

  1. Is Training is mandatory to real time with real time training datasets ? (or) is it okay to use the kitti dataset used already in test algorithm and use the same training parameters.Please let us know on your experience.
  2. If we have to train the network with our own data, please know us how to prepare the train dataset format and hot to feed to network for training. I tried to prepare the train dataset with GPS and IMU. GPS is taken as ground truth but lost in how to train the network. Lost in train_torch_filter.py and dataset.py in modifying the dataset class to my own sequence.
  3. Any CUDA version needed for training own sequence?

Your suggestions and inputs on these queries would be great and very much helpful. Also if created, Please could you share the github repository for real time implementation of algorithm, that would be really helpful and solve my other thousand questions.

Best Regards, Harish

sharish33 avatar Mar 26 '20 10:03 sharish33