RossHartley / invariant-ekf

C++ library to implement invariant extended Kalman filtering for aided inertial navigation.
BSD 3-Clause "New" or "Revised" License
460 stars 101 forks source link

Derivation of left error derivative with noise. #11

Closed ghost closed 2 years ago

ghost commented 2 years ago

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.

RossHartley commented 2 years ago

Only the dynamics of X are assumed to have noise. d/dt Xbar does not have noise.

ghost commented 2 years ago

Thanks!