没有合适的资源?快使用搜索试试~ 我知道了~
首页Invariant Kalman
资源详情
资源评论
资源推荐
AS01CH10_Bonnabel ARI 10 April 2018 9:35
Annual Review of Control, Robotics, and
Autonomous Systems
Invariant Kalman Filtering
Axel Barrau
1
and Silv
`
ere Bonnabel
2
1
Safran Tech, Groupe Safran, 78772 Magny Les Hameaux CEDEX, France;
email: axel.barrau@safrangroup.com
2
MINES ParisTech, PSL Research University, Centre for Robotics, 75006 Paris,
France; email: silvere.bonnabel@mines-paristech.fr
Annu. Rev. Control Robot. Auton. Syst. 2018.
1:237–57
First published as a Review in Advance on
December 20, 2017
The Annual Review of Control, Robotics, and
Autonomous Systems is online at
control.annualreviews.org
https://doi.org/10.1146/annurev-control-060117-
105010
Copyright
c
2018 by Annual Reviews.
All rights reserved
Keywords
Kalman filter, geometry, Lie groups, localization, autonomous navigation,
simultaneous localization and mapping, SLAM
Abstract
The Kalman filter—or, more precisely, the extended Kalman filter (EKF)—
is a fundamental engineering tool that is pervasively used in control and
robotics and for various estimation tasks in autonomous systems. The re-
cently developed field of invariant extended Kalman filtering uses the geo-
metric structure of the state space and the dynamics to improve the EKF,
notably in terms of mathematical guarantees. The methodology essentially
applies in the fields of localization, navigation, and simultaneous localization
and mapping (SLAM). Although it was created only recently, its remarkable
robustness properties have already motivated a real industrial implementa-
tion in the aerospace field. This review aims to provide an accessible in-
troduction to the methodology of invariant Kalman filtering and to allow
readers to gain insight into the relevance of the method as well as its impor-
tant differences with the conventional EKF. This should be of interest to
readers intrigued by the practical application of mathematical theories and
those interested in finding robust, simple-to-implement filters for localiza-
tion, navigation, and SLAM, notably for autonomous vehicle guidance.
237
Click here to view this article's
online features:
• Download figures as PPT slides
• Navigate linked references
• Download citations
• Explore related articles
• Search keywords
ANNUAL
REVIEWS
Further
Annu. Rev. Control Robot. Auton. Syst. 2018.1:237-257. Downloaded from www.annualreviews.org
Access provided by 2603:3024:604:3100:d1d0:18e1:f019:5c07 on 08/31/18. For personal use only.
AS01CH10_Bonnabel ARI 10 April 2018 9:35
1. INTRODUCTION
The goal of a filter is to estimate the state of a dynamical system by combining an evolution model
and some sensor measurements that bring partial information about the state. Unfortunately,
models are inherently inaccurate, and sensors are subject to noise that corrupts the measurements.
The idea of filtering is to explicitly include both sources of uncertainty in the model and compute
the best estimates of the state that can be inferred from the available information.
Even though the mathematical theory is now well understood, it is still challenging to design
filters in practice for control, robotics, and autonomous systems. The extended Kalman filter
(EKF) appeared in the 1960s with the advent of computers and was first implemented by NASA
in the Apollo program to estimate the trajectory of the space capsule in real time. The past
two decades have also witnessed the development of particle filters, with great advances in both
theory and practice. However, these filters rely on extensive numerical computations that are not
always suited to real-time onboard implementations, and there are theoretical caveats, especially
when process noise is low (typically for static parameter estimation). The robotics community has
also more recently turned to optimization-based techniques for filtering (the problem is typically
formulated as nonlinear least squares), but the computational demands are extensive, and their
robustness to erroneous initializations is not yet clearly established. Because robots and many
control systems are real-time systems, the amount of computation is limited, and the EKF is still a
widespread tool in control and robotics, along with its more recent variant, the unscented Kalman
filter (UKF). In the aerospace industry, the EKF remains the reference fi lter.
1.1. Extended Kalman Filtering
Consider a general dynamical system in discrete time whose state is described by the vector variable
X
n
∈ R
d
. We associate a sequence of observations (Y
n
)
n0
∈ R
p
, which are measurement data
returned by sensors. The trusted evolution model is
X
n
= f (X
n−1
, u
n
, w
n
), 1.
where f is the function encoding the evolution of the system; w
n
is the (unknown) process noise,
which is a centered random variable with covariance matrix Q
n
; the vector u
n
∈ R
m
is a control
input; and the observation consists of partial measurements of t he state at time n:
Y
n
= h(X
n
) + V
n
,2.
with h the observation function and V
n
the (unknown) measurement noise that accounts for
sensors’ limitations.
The EKF computes in real time an approximation
ˆ
X
n|n
to the best state estimate given the
observations. Let Y
1:n
denote the collection of past measurements Y
1
, Y
2
, ..., Y
n
, and let u
1:n
be
similarly defined. To be more precise, the EKF represents the belief P(X
n
| u
1:n
, Y
1:n
), which
assigns a probability to each possible value of the true state in light of all the information collected
so far, by a mean
ˆ
X
n|n
and covariance matrix P
n|n
. Indeed, the rationale is to use the following
Gaussian approximation: P(X
n
| u
1:n
, Y
1:n
) ≈ N (
ˆ
X
n|n
, P
n|n
). To compute the mean and covariance,
the EKF uses a two-step procedure:
1. Propagation : The estimate
ˆ
X
n−1|n−1
obtained after the observation Y
n−1
is propagated through
the deterministic part of Equation 1:
ˆ
X
n|n−1
= f (
ˆ
X
n−1|n−1
, u
n
,0). 3.
238 Barrau
·
Bonnabel
Annu. Rev. Control Robot. Auton. Syst. 2018.1:237-257. Downloaded from www.annualreviews.org
Access provided by 2603:3024:604:3100:d1d0:18e1:f019:5c07 on 08/31/18. For personal use only.
AS01CH10_Bonnabel ARI 10 April 2018 9:35
To compute the associated covariance, introduce the estimation errors defined as
e
n−1|n−1
= X
n−1
−
ˆ
X
n−1|n−1
, e
n|n−1
= X
n
−
ˆ
X
n|n−1
. 4.
The key idea underlying the EKF is to linearize the error system through a first-order
Taylor expansion of the nonlinear functions f and h at the estimate
ˆ
X
n−1|n−1
. Indeed, using
the Jacobians F
n
=
∂ f
∂ X
(
ˆ
X
n−1|n−1
, u
n
,0),G
n
=
∂ f
∂w
(
ˆ
X
n−1|n−1
, u
n
,0),and H
n
=
∂h
∂ X
(
ˆ
X
n|n−1
), the
combination of Equations 1–3 yields the following first-order expansion of the error system:
e
n|n−1
= F
n
e
n−1|n−1
+ G
n
w
n
,5.
Y
n
− h(
ˆ
X
n|n−1
) = H
n
e
n|n−1
+ V
n
,6.
where the second-order terms—that is, terms of order O
||e||
2
, ||w||
2
, ||e||||w||
—have
been removed (see, e.g., Reference 1). P
n−1|n−1
is an approximation to the true covari-
ance E(e
n−1|n−1
e
T
n−1|n−1
), and it is propagated through the linearized model Equation 5 so
that P
n|n−1
= F
n
P
n−1|n−1
F
T
n
+ G
n
Q
n
G
T
n
is an approximation of E(e
n|n−1
e
T
n|n−1
), and we have
P(X
n
| u
1:n
, Y
1:n−1
) ≈ N (
ˆ
X
n|n−1
, P
n|n−1
).
2. Measurement update: To account for the measurement Y
n
,weletz
n
= Y
n
− h(
ˆ
X
n|n−1
), and
z
n
is called the innovation. Assuming that e
n|n−1
∼ N (0, P
n|n−1
) and that the approximation
from Equation 6 is exact, the linear Kalman filter equations ensure that the updated error
e
n|n
= X
n
−
ˆ
X
n|n
satisfies e
n|n
∼ N (0, P
n|n
), where
ˆ
X
n|n
=
ˆ
X
n|n−1
+ K
n
z
n
and P
n|n
= [I − K
n
H
n
]P
n|n−1
,7.
with K
n
, called the Kalman gain, computed in Algorithm 1. Of course, the belief after
update N (
ˆ
X
n|n
, P
n|n
) is only an approximation to P(X
n
| u
1:n
, Y
1:n
) owing to the linearizations.
In practice, those linearizations may lead the filter to inconsistencies and sometimes even
divergence.
Algorithm 1 (extended Kalman filter). Choose an initial estimate
ˆ
X
0|0
and uncertainty matrix P
0|0
.
loop
Define F
n
, G
n
,andH
n
through Equations 5 and 6
Define Q
n
as Cov(w
n
)andR
n
as Cov(V
n
)
Propagation
ˆ
X
n|n−1
= f (
ˆ
X
n−1|n−1
, u
n
,0)
P
n|n−1
= F
n
P
n−1|n−1
F
T
n
+ G
n
Q
n
G
T
n
Measurement update
Compute z
n
= Y
n
− h(
ˆ
X
n|n−1
), S
n
= H
n
P
n|n−1
H
T
n
+ R
n
,and
K
n
= P
n|n−1
H
T
n
S
−1
n
P
n|n
= [I − K
n
H
n
]P
n|n−1
ˆ
X
n|n
=
ˆ
X
n|n−1
+ K
n
z
n
end loop
1.2. Motivation for the Use of Geometry
Users who are facing the filtering problem defined by the system of Equations 1 and 2, with f
and h two nonlinear maps, are free to choose a different coordinate system to design an EKF.
For instance, in radar tracking, one can choose not only a frame attached to the target or to the
radar, but also range and bearing as an alternative to Cartesian coordinates. In general, it is unclear
what the best coordinates for EKF design are. However, some systems have a natural choice of
www.annualreviews.org
•
Invariant Kalman Filtering 239
Annu. Rev. Control Robot. Auton. Syst. 2018.1:237-257. Downloaded from www.annualreviews.org
Access provided by 2603:3024:604:3100:d1d0:18e1:f019:5c07 on 08/31/18. For personal use only.
AS01CH10_Bonnabel ARI 10 April 2018 9:35
coordinates. For instance, when facing a linear Gaussian system, the EKF boils down to the linear
Kalman filter, which is optimal, and it would be nonsense to work with alternative coordinates
that would make the system nonlinear. In this article, we advocate that, for a large class of systems
defined on matrix Lie groups, the machinery of geometry provides coordinates that are unarguably
more suited to the problem. In those cases, the invariant extended Kalman filter (IEKF) theory is
useful, as the original problem is often formulated using coordinates that do not match the group
structure, leading to degraded performance of the conventional EKF.
For systems on Lie groups, the IEKF was originally introduced in Reference 2 and continued
in References 3–7. Reference 7 recently described the complete methodology along with the
convergence properties of the IEKF. More generally, the use of Lie groups for state estimation
dates back to the 1970s (8–10) and has recently spanned a range of applications and a rich stream
of theoretical results (see, e.g., 6, 11–16).
Following the work of, among others, Chirikjian (17), the robotics community has increasingly
recognized that using probability distributions properly defined on Lie groups is important, no-
tably for pose estimation (see, e.g., 17–22). Moreover, Reference 23 recently showed that the use of
the IEKF over the conventional EKF solves the inconsistency issues of EKF-based simultaneous
localization and mapping (SLAM).
1.3. Outline
Section 2 consists of geometry preliminaries. Section 3 reviews the methodology of invariant
Kalman filtering. Section 4 is concerned with the mathematical guarantees that come with the
IEKF. Section 5 presents some real industrial applications in the field of inertial navigation.
Section 6 reviews the inconsistency of EKF-based SLAM and the interest of IEKF-based SLAM.
The presentation of the present article is freely inspired by a tutorial article that uses methods
rooted in differential geometry to improve Monte Carlo schemes (24).
2. LIE GROUPS AND PROBABILITY
2.1. Matrix Lie Groups
1
In this section, we provide readers with the bare minimum of Lie group theory that is required
to introduce the IEKF methodology. A matrix Lie group G is a subset of square invertible N × N
matrices M
N
(R) verifying the following properties:
I
N
∈ G, ∀χ ∈ G, χ
−1
∈ G, ∀χ
1
, χ
2
∈ G, χ
1
χ
2
∈ G,
where I
N
is the identity matrix of R
N
. The subset G is generally not a vector space and can thus be
viewed as a curved space (see Figure 1). To every point χ ∈ G, one can associate a vector space
T
χ
G that is called the tangent space at χ and is defined as all the matrices that write
d
dt
γ (0), where
γ : R → G is a smooth curve of G that satisfies γ (0) = χ . The elements of this space are called
tangent vectors.
The tangent space T
I
N
G at the identity I
N
is called the Lie algebra and plays a very specific
role. It is denoted g, and its dimension d defines a dimension for the group G itself, where d
is generally much smaller than the dimension N
2
of the ambient space. We have g ⊂ M
N
(R),
1
All of the results carry over to general abstract Lie groups, but the (less general) matrix Lie groups are well suited to tutorial
and computational purposes and encompass all of the applications discussed.
240 Barrau
·
Bonnabel
Annu. Rev. Control Robot. Auton. Syst. 2018.1:237-257. Downloaded from www.annualreviews.org
Access provided by 2603:3024:604:3100:d1d0:18e1:f019:5c07 on 08/31/18. For personal use only.
AS01CH10_Bonnabel ARI 10 April 2018 9:35
T
I
N
G
T
χ
G
G
G
χ
χ
I
N
d
Figure 1
G is a curved space. Left and right multiplications offer two ways to identify the tangent space T
χ
G at χ with
the tangent space at identity T
I
N
G, called the Lie algebra g. In turn, the application ξ → ξ
∧
provides a linear
bijection between the Euclidean space R
d
and g. (Lie groups are homogeneous spaces, which somehow “look
the same everywhere.” As such, the figure may be slightly misleading, since the curved surface representing
G seems irregular. Yet it seems to us that representing a Lie group by, e.g., a sphere would be an
oversimplification and to some extent equally misleading.)
but there is always an invertible map R
d
→ g that allows identifying g to R
d
.Forξ ∈ R
d
,we
denote ξ
∧
∈ g the corresponding element of g, and we recall that ξ → ξ
∧
is a linear map. There
are then two canonical ways to identify R
d
and the tangent space T
χ
G at any χ ∈ G: through
left and right multiplications, which are generally different. Indeed, for any ξ ∈ R
d
, the vectors
χ(ξ
∧
)and(ξ
∧
)χ are both tangent vectors at χ. Of course, they are generally different owing to
the noncommutativity of matrix multiplication.
The usual matrix exponential map exp
m
: g → G constitutes a bijection from a n eighborhood
V ⊂ g of 0 to a neighborhood of the identity I
N
in G. In this article, we call the Lie exponential map
the map exp : R
d
→ G,definedbyexp(ξ) = exp
m
(ξ
∧
), which is a bijection in a neighborhood of
0 ∈ R
d
, with exp(ξ
∧
)
−1
= exp(−ξ
∧
). Moreover, χ exp(·)andexp(·)χ provide two distinct bijections
between a neighborhood of 0 in R
d
and a neighborhood of χ in G.
The Baker-Campbell-Hausdorff (BCH) formula gives a series expansion for the image in g
of the product on G:BCH(ξ , ζ ) = exp
−1
(exp(ξ)exp(ζ )). In particular, it ensures exp(ξ )exp(ζ ) =
exp(ξ + ζ + T ), where T is of the order O(ξ
2
, ζ
2
, ξζ ).
Example 1. Consider the group of rotation matrices G = SO(3), which describes the
orientation (attitude) of a body in space. It is the subset of matrices R of M
3
(R) such that
RR
T
= I
3
and det(R) = 1. Each R ∈ G can be viewed as the rotation that maps vectors
expressed in the body frame to vectors expressed in the earth-fixed frame. We have
d = 3, and for any ξ ∈ R
3
, ξ
∧
∈ R
3×3
is the skew symmetric matrix associated with the
cross-product operator ζ → ξ ×ζ . The Lie algebra is the set of skew symmetric matrices.
Any tangent vector U ∈ T
R
G at R represents an infinitesimal shift of R in SO(3). It can
be written as U = Rξ
∧
for some vector ξ ∈ R
3
or, alternatively, as U = ζ
∧
R for some
ζ ∈ R
3
. Both ξ and ζ can be seen as angular velocity vectors that represent the same
infinitesimal rotation, but ξ is a vector of the body frame, whereas ζ is expressed in the
earth-fixed frame. It is easy to prove that indeed ζ = Rξ .
2.2. Uncertainty Representation on Matrix Lie Groups
To define random variables on Lie groups, we cannot apply the usual approach of additive noise
for χ ∈ G because G is not a vector space. By contrast, we define the probability distribution
www.annualreviews.org
•
Invariant Kalman Filtering 241
Annu. Rev. Control Robot. Auton. Syst. 2018.1:237-257. Downloaded from www.annualreviews.org
Access provided by 2603:3024:604:3100:d1d0:18e1:f019:5c07 on 08/31/18. For personal use only.
剩余22页未读,继续阅读
Una_una
- 粉丝: 0
- 资源: 3
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- 27页智慧街道信息化建设综合解决方案.pptx
- 计算机二级Ms-Office选择题汇总.doc
- 单链表的插入和删除实验报告 (2).docx
- 单链表的插入和删除实验报告.pdf
- 物联网智能终端项目设备管理方案.pdf
- 如何打造品牌的模式.doc
- 样式控制与页面布局.pdf
- 武汉理工Java实验报告(二).docx
- 2021线上新品消费趋势报告.pdf
- 第3章 Matlab中的矩阵及其运算.docx
- 基于Web的人力资源管理系统的必要性和可行性.doc
- 基于一阶倒立摆的matlab仿真实验.doc
- 速运公司物流管理模式研究教材
- 大数据与管理.pptx
- 单片机课程设计之步进电机.doc
- 大数据与数据挖掘.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论0