The invariant extended Kalman filter (IEKF) (not to be confused with the iterated extended Kalman filter) was first introduced as a version of the extended Kalman filter (EKF) for nonlinear systems possessing symmetries (or invariances),[1] then generalized and recast as an adaptation to Lie groups of the linear Kalman filtering theory.[2] Instead of using a linear correction term based on a linear output error, the IEKF uses a geometrically adapted correction term based on an invariant output error; in the same way the gain matrix is not updated from a linear state error, but from an invariant state error. The main benefit is that the gain and covariance equations have reduced dependence on the estimated value of the state. In some cases they converge to constant values on a much bigger set of trajectories than is the case for the EKF, which results in a better convergence of the estimation.
Consider a system whose state is encoded at time step
n
xn
G
xn=\phin(xn-1) ⋅ un
\phin
G
⋅
un
G
yn
yn=xn*bn
bn
l{Y}
G
*
l{Y}
G
yn=bn*xn
The invariant extended Kalman filter is an observer
\hat{x}n
\hat{x}n|n-1=\phin(\hat{x}n-1|n-1) ⋅ un
\hat{x}n|n=\hat{x}n|n-1 ⋅ \exp(Kn
-1 | |
(\hat{x} | |
n|n-1 |
*yn-bn))
\exp
G
Kn
If measurement function is a right action then the update state is defined as:
\hat{x}n|n=\exp(Kn(yn*
-1 | |
\hat{x} | |
n|n-1 |
-bn)) ⋅ \hat{x}n|n-1
The discrete-time framework above was first introduced for continuous-time dynamics of the shape:
d | |
dt |
xt=ft(xt),
ft
t
\foralla,b\inG,ft(a ⋅ b)=ft(a)b+aft(b)-af(Id)b
Id
gu=Lg(u)
ug=Rg(u)
Lg:TGx → TGgx
Rg:TGx → TGxg
TGx
G
x
The main benefit of invariant extended Kalman filtering is the behavior of the invariant error variable, whose definition depends on the type of measurement. For left actions we define a left-invariant error variable as:
L | |
e | |
n|n-1 |
=
-1 | |
\hat{x} | |
n|n-1 |
xn
L | |
e | |
n|n |
=
-1 | |
\hat{x} | |
n|n |
xn
R | |
e | |
n|n-1 |
=xn
-1 | |
\hat{x} | |
n|n-1 |
R | |
e | |
n|n |
=xn
-1 | |
\hat{x} | |
n|n |
xn
\hat{x}n|n-1
\hat{x}n|n
L | |
e | |
n|n-1 |
=
-1 | |
u | |
n |
⋅
L | |
\phi | |
n-1|n-1 |
) ⋅ un
L | |
e | |
n|n |
=
L | |
\exp(-K | |
n|n-1 |
*bn-bn)) ⋅
L | |
e | |
n|n-1 |
R | |
e | |
n|n-1 |
=
R | |
\phi | |
n-1|n-1 |
)
R | |
e | |
n|n |
=
R | |
e | |
n|n-1 |
\exp(-Kn(bn*
R | |
e | |
n|n-1 |
-bn))
\xin|n-1,\xin|n
en|n-1=\exp(\xin|n-1),
en|n=\exp(\xin|n).
Fn
\xin|n=Fn\xin|n-1+\circ(||\xin|n-1||)
\xin|n=Fn\xin|n-1.
\xin|n,\xin|n-1
Most physical systems possess natural symmetries (or invariance), i.e. there exist transformations (e.g. rotations, translations, scalings) that leave the system unchanged. From mathematical and engineering viewpoint, it makes sense that a filter well-designed for the considered system should preserve the same invariance properties. The idea for the IEKF is a modification of the EKF equations to take advantage of the symmetries of the system.
Consider the system
x |
{=}f(x,u)+M(x)w
y{=}h(x,u)+N(x)v
where
w,v
G
e
\varphig,\psig,\rhog
g\inG
(X,U,Y)=(\varphig(x),\psig(u),\rhog(y))
\varphig,\psig,\rhog
X{=}f(X,U)+M(X) |
w
Y{=}h(X,U)+N(X)v
Since it is a symmetry-preserving filter, the general form of an IEKF reads[4]
\hatx |
=f(\hatx,u) +W(\hatx)Ll(I(\hatx,u),E(\hatx,u,y)r)E(\hatx,u,y)
where
E(\hatx,u,y)
\haty-y
W(\hatx)=l(w1(\hatx),..,wn(\hatx)r)
I(\hatx,u)
L(I,E)
To analyze the error convergence, an invariant state error
η(\hatx,x)
\hatx-x
Given the considered system and associated transformation group, there exists a constructive method to determine
E(\hatx,u,y),W(\hatx),I(\hatx,u),η(\hatx,x)
Similarly to the EKF, the gain matrix
L(I,E)
L{=}PCTl(N(e)NT(e)r)-1
P{=}AP+PA |
T+M(e)MT(e)-PCTl(N(e)NT(e)r)-1CP
where the matrices
A,C
I(\hatx,u)
(\hatx,u)
A,C
Invariant extended Kalman filters are for instance used in attitude and heading reference systems. In such systems the orientation, velocity and/or position of a moving rigid body,e.g. an aircraft, are estimated from different embedded sensors, such as inertial sensors, magnetometers, GPS or sonars. The use of an IEKF naturally leads[5] to consider the quaternion error
\hatqq-1
A major application of the Invariant extended Kalman filter is inertial navigation, which fits the framework after embedding of the state (consisting of attitude matrix
R
v
x
SE2(3)
(R1,v1,x1) ⋅ (R2,v2,x2)=(R1R2,x1+R1x2,v1+R1v2)
The problem of simultaneous localization and mapping also fits the framework of invariant extended Kalman filtering after embedding of the state (consisting of attitude matrix
R
x
p1,...,pK
SEK+1(3)
SEK+1(2)
(R1,x1,
1, | |
p | |
1 |
...,
K) | |
p | |
1 |
⋅ (R2,x2,
2, | |
p | |
2 |
...,
K) | |
p | |
2 |
=(R1R2,x1+R1x2,
1 | |
p | |
1 |
+R1
1, | |
p | |
2 |
...,
K | |
p | |
1 |
+R1
K) | |
p | |
2 |