rosshartley / invariant-ekf Goto Github PK

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

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!

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.

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!

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!

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.

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, 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

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.