rosshartley / invariant-ekf Goto Github PK
View Code? Open in Web Editor NEWC++ library to implement invariant extended Kalman filtering for aided inertial navigation.
License: BSD 3-Clause "New" or "Revised" License
C++ library to implement invariant extended Kalman filtering for aided inertial navigation.
License: BSD 3-Clause "New" or "Revised" License
Hi doctor RossHartley,
When i derive the error state differential equation of your reference [7] "Axel Barrau and Silvère Bonnabel. The invariant extended
Kalman filter as a stable observer. IEEE Transactions on Automatic Control, 62(4):1797–1812, 2017", the equation (30). i got a different forms. Is there something i take a mistake or is it a typo in equ(30) of paper?
Update
I notice when i ignore noise in the estimation state dynamics, i can get the same left error state dynamics with the author.
In the right error state dynamics, the last term in my derivation is -eta * xt * wt * xt^(-1)
, is equal to the last term in author's right error state dynamics is -xt^(hat) * wt * xt^(hat) ^(-1) * eta
.
My problem is solved, close it now!
filter.setState() fails when compiled with MUTEX option
Hi, I'm trying to reproduce your results on my own to make sure that I really understand your paper Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation.
When I look the kinematics.cpp, I find the quaternion of the imu_kinematic_measurements.txt is used in
q = Eigen::Quaternion<double>(stod98(measurement[i + 1]), stod98(measurement[i + 2]), stod98(measurement[i + 3]), stod98(measurement[i + 4]));
I do not understand where it means rotation from where to where. This is really strange to me. I would be really appreciated if you can answer this question for me.
Thank you so much in advance for your time!
Best
Sun
Hi, thanks a lot for this code. I am working on a multisensor state estimator for quadruped robots, and I would like to use your inEKF as a benchmark. Still, I need to add lidar measurements (position and rotation), to correct the state together with the kinematics measurements.
I'm currently defining the measurement vector as y=[y_leg, y_lid_transf], where y_lid_transf includes information on lidar rotation and position. I am not sure if this strategy will work because I'm still not obtaining good improvements, so if you have any suggestions, I will be very grateful.
Hi, I'm trying to reproduce your results on my own to make sure that I really understand your paper Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation*
. When I look into the KINEMATIC
measurement in imu_kinematic_measurements.txt
, I find it is quite strange.
Here is my understanding: instead of measuring the joint encoder of the Cassie, the imu_kinematic_measurements.txt
provides the relative position between the foot and the robot directly, which I can understand. However, The imu_kinematic_measurements.txt
also provides the covariance matrix, which I really don't understand. Can the covariance matrix be measured? This is really strange to me. I would be really appreciated if you can answer this question for me.
Thank you so much in advance for your time!
Best
Yuan
Trying to derive left error with noise (equation 30 in paper - left version). Similar to how paper does noise free derivative of error.
d/dt n = g(n) - w^ n <- this is what we seek and is the left version of equation 30 in the paper.
Definitions:
d/dt x = f(x) + x w^
n = x^-1 xbar
x n = xbar
g(n) = f(n) - f(I) n
Start by taking derivative:
d/dt(n) = d/dt (x^-1) xbar + x^-1 d/dt xbar
Substitute d/dt(x ^x-1) = d/dt I = 0 -> d/dt x^-1 = -x^-1 (d/dt x) x^-1
= -x^-1 d/dt x x^-1 xbar + x^-1 d/dt xbar
Insert dynamics "f":
= -x^-1 (f(x) + x w^) x^-1 xbar + x^-1 (f(xbar) + xbar w^)
= -x^-1 f(x)x^-1 xbar + -x^-1 x w^ x^-1 xbar + (x^-1 f(xbar) + x^-1 xbar w^)
Put n back in
= -x^-1 f(x)n -w^ n + x^-1 f(xbar) + n w^
Eliminte xbar
= -x^-1 f(x)n -w^ n + x^-1 f(x n) + n w^
Set x = I
= -f(I)n -w^ n + f(n) + n w^
Insert defintion of g
= f(n) - f(I)n -w^ n + n w^
= g(n) -w^ n + n w^
Almost but not quite. I can't get rid of "n w^". What am I missing? Paper derives noise free error derivative but only states results of the noisey version.
hello,When I run simulink, I meet the following problems.
Simulink cannot propagate the variable-size mode from the 'Output Port 1' of 'RIEKF_test/Landmark Spoofer' to the 'Input Port 1' of 'RIEKF_test/Orientation EtherCAT Rate Transition6'. This input port expects a fixed-size mode. The variable-size mode originates from 'RIEKF_test/Landmark Spoofer/MATLAB Function1'. Examine the configurations of 'RIEKF_test/Orientation EtherCAT Rate Transition6' for one of the following scenarios: 1) the block does not support variable-size signals; 2) the block supports variable-size signals but needs to be configured for them.
The IEKF system noise Qt=Adx*Cov (wt)*Adx^T .
where
Adx=[R 0 0 0]
[(vt)xR R 0 0]
[(pt)xR 0 R 0]
[(dt)xR 0 0 R]
if the position pt is large, for example the robot moves to a certain location (1000,1000,1000),and its velocity is (100,100,100) ,then Qt is very large. Namely IEKF's system noise is very large and this is unreasonable. Why is this happening?
Hi, thanks for making this code available.
I am trying to use it with a pre determined map of ceiling mounted apriltag markers. (corner positions known).
Would you be able to give me a few tips on how to use your example and add marker corners?
Would I give each corner an id , and set them as landmarks?
Where is the camera / robot pose added?
Thanks!
Hello,
I want to implement ZUPT (zero velocity updates). I first thought it would be a piece of cake but I came to the realisation that I have to master the theory first... which is not out of reach, but certainly going to take least a couple of days. Still, the solution might be really simple so I'm taking a chance here. Perhaps someone could provide the solution in just a few key lines of code.
Thanks to anyone!
And a big thanks to Ross Hartley for sharing.
Has this repository compiled on Mac? I'm using Mac to compile this repository and I got following error message:clang: error: linker command failed with exit code 1 (use -v to see invocation)
make[2]: *** [../lib/libinekf.dylib] Error 1
make[1]: *** [CMakeFiles/inekf.dir/all] Error 2
make: *** [all] Error 2
I have installed the requirement package already.
Thanks in advance!
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.