对偶四元数导航算法与非线性高斯滤波分析研究
儿歌童谣-试用期工作总结报告
附件6
作者姓名：武元新
论文题目：对偶四元数导航算法与非线性高斯滤波研究
作者简介：
：
武元新
，男，1976年11月出生，2000年04月师从于国防科学
技术大学胡德文教授，于2005年1
2月获博士学位。
中 文 摘 要
本论文深入研究了导航系统
中所涉及的两个基本理论问题：导航信息的数学描述和数值解算；导航
目前，导航、机器人和
计算机视觉等研究领域普遍以向量代数为工具研究空间运动学问题。但是当
这意味着我们可以转而采用另
一种方式处理转动和平移。在导航领域中，方向余弦矩阵DCM或四元数用
，也就是说，可以通过一个简
单的数学公式把圆锥算法变换成相应的划船算法，但是导航算法的设计和
惯性导航本质上要解决的是一个三维空间的刚体运动学问题。
存在统一、简洁地描
述一般性刚体运动的数学语言吗？如果存在，可否利用该数学工具设计思路明
论文的前半部分在建立基于对偶四元数的捷联惯性导航系统理论方面作了一些探索性工作。
1. 作为几何代数的子集，对偶四元数是刻画一般性刚体运动的最简洁、最有效的数学工具，可以用来
Itzhack分离坐标系的思想，论文第二章运用对偶四元数代数重新诠释捷联式惯性导航的基本原理
，
于对偶四元数的捷联惯性导航算法。对偶四元数算法将传统算法中的圆锥、划船和卷轴修正整合到算法和划船算法之间存在对偶性等价性的根本原因，导出了对偶四元数算法和传统算法误差的解读
析
提供了强有力的佐证。对高精度导航系统和大机动场景来说，对偶四元数算法是一个更好的选择
<已发表
于IEEE Trans. o
Estimation by Fusing Inertial 2006）最近，Ohio大学的Soloviev博士在频域而不是时域中实现了传统导航算法。据称，该
频域方法
学基金资助
,
“频域中的对偶四元数捷联惯性导航算法研究”，606040
11）
。
2.
3.
在基于对偶四元数的捷联惯性导航理论框架中，姿态
、速度和位置等所有的导航参数都可以从三个
达的误差模型：加性对偶四元数误差模型和乘性对偶四元数
误差模型。这两个误差模型可用来搭建
对偶四元数导航算法直接输出的导航参数是在地球坐标系中表达的
，但是在GPS导航和测地学等应用
Raphson方法的坐标变换快速算法。除了靠近地心的一个小区
域之外，新算法不存在奇异和不收敛的
Systems, 2003）
1 7
作为一种完全自主的航迹推算方法，惯性导航存在一个固有缺陷，即其误差随着时间累计。为了克
提供的互补信息来提高导航系统的精度和冗余度的技术。无论采用何种配置<惯性导航、卫星导
航、雷达
这正是非线性滤波器在组合导航系统中所起的作用，即非线性滤波是组合导航系统中信息融合的
基石。
EKF）本身就是在组合导航系统需求牵引下的研究成果。自从20世纪60年代在阿波罗计划中
首次实现以
的非线性系统模型进行线性化，滤波过程中容易发散等等。近年来，非线性滤波研究取得了众
多的成果
那么，可否找到或设计出替代EKF的通用非线性滤波器？从信息论的角度讲，这是
一个寻找比EK
4.
论文的后半部分着重研究了非线性高斯滤波问题。
Bayes推演为动态系统的状态估计问题提供了最优的解决方案，但是由于其最优解需要传播整个概率
滤波器，其背景也迥然不同。面对众多的高斯滤波器，如何确定哪一种最适合用来解决手上的滤波
ss-Hermite积分公式，单项式精确公式和函数拟合方法）对一般形式的高斯滤波器的近似。基于多维
5.
近似高斯滤波器奠定了基础，对运用更好的积分方法设计高效、稳定的滤波器具有一定的
指导意义
6.
论文第六章研究了UKF
下使用非扩展
形式的UKF很可能会损失滤波精度。本章首先导出了扩展UT与非扩展UT等价的前提条
噪声的影响。
这个区别通常有利于扩展UKF，因为奇次矩被变换后的sigma点集捕获，并得以在单个
真实例的结
果与分析结论一致。<已发表于
最近提出的高斯粒子滤波器
，验
后概率密度可以假定为容易采样的任意分布，比如混合高斯。这种情况下，QuasiGPF可以用来
C
arlo方法可用于近似计算多维积分或序贯Bayes概率推演。我们基于准Monte
Carlo方法研究了一类特
关键词：惯性导航, 对偶四元数, 数值积分, 误差特性,
坐标变换,
非线性高斯滤波, 粒子滤波, 低偏差序列
Dual-Quaternion
Navigation Algorithm and Nonlinear Gaussian
Filtering
Wu Yuanxin
2 7
ABSTRACT
The thesis has
investigated two theoretical problems involved in
navigation systems. One is the
mathematical
representation and computation of navigation
information。 the other is the optimal
real-
time fusion strategy, i.e., filtering, of
navigation information.
So far in most,
if not all, fields, researches have been made
within the framework of vector
algebra. When
it comes to the general displacement of a rigid
body, however, there is no such a
mathematical
tool in vector algebra as to treat rotation and
translation simultaneously. Fortunately,
from
a viewpoint of kinematicsthe general displacement
can be taken apart into two separate
motions,
i.e. fixed-point rotation and translation, in
which rotation is completely independent of
translation. This means that we can otherwise
treat rotation and translation in a different
manner. In
the navigation community,
DCMquaternion and vector are chosen to represent
rotation and
translation, respectively. So are
the strapdown INS algorithms. Individualalgorithm
as mentioned
above has to be structured for
attitude integration and velocityposition
integration, respectively.
The characteristics
of dualityequivalence between the coning and
sculling algorithms were revealed
recently,
but the algorithm design and implementation are
still rather involved.
In essence,
inertial navigation is to solve the kinematic
problem of a three-dimensional rigid
body.
Is there any mathematical language that
represents rotation and translation in a
consolidating and compact manner? Is it
possible to reduce the perplexing and error-prone
strapdown algorithmsto some extent? The
answers to these questions are both affirmative.
The first half of this thesis has done
some pilot works on the strapdown inertial
navigation theory
founded on dual quaternion.
1. As a subset of geometry algebra, dual
quaternion is the most concise and efficient
mathematical tool to represent the general
rigid motion. It can be used to address all rigid
kinematic (and dynamic> problems, including
inertial navigation of course. Benefiting from
Bar-Itzhack’s split-coordinatescheme, Chapter
2 reinterprets the rationale of the strapdown
inertial navigation in terms of dual
quaternion algebra, obtaining three dual
quaternion
kinematic equations that take the
same forms as the conventional attitude quaternion
rate
equation. Borrowing the traditional two-
speed approach originally developed in
conventional
attitude integration, we design
one new numerical integration algorithm to solve
the three
kinematic equations, thus obtaining
the dual quaternion navigation algorithm,which
integrates
the coning, sculling and scrolling
corrections all together, considerably simplifying
the
algorithm structure and implementation
new navigation algorithm is analyzed
and
compared with the conventional one from various
aspects. It is shown that screw motion
3 7
2.
3.
in itself consists of
coning motion and sculling motion
.
The
duality between the coning and
sculling
corrections, raised in the recent literature, is
fundamentally explained. The superiority
of
the new algorithm in accuracy is analytically
derived. A variety of simulations are carried
out to support the analytic conclusions,
including those with ideal inertial sensors and
those
with non-ideal ones. The numerical
results agree well with the analyses. The new
algorithm
turns out to be a better choice than
the conventional algorithm for high-precision
navigation
systems and high-maneuver
applications. Several guidelines in choosing a
suitable navigation
algorithm are also
provided according to the inertial sensors
configuration and the turning
frequency of the
conventional the near future, the dual quaternion
algorithm is
expected to take an important
role in ultra-cold atom interferometry based
precision inertial
navigation systems.
(Published in
dual quaternions,IEEE Trans. on
Aerospace and Electronic Systems, 2005；On
mathematical framework for strapdown algorithm
designJournal of Guidance, Control, and
Dynamics, 2006。 Analysis of Rotation
Estimation by Fusing Inertial and
Line-Based
Visual Information: A Revisit,Automatica,
2006>Recently, Dr. Soloviev in
OhioUniversityhas implemented the conventional
strapdown algorithm in frequency domain
rather
than in the customary time domain. It is claimed
that the frequency-domain approach
has a
significant improvement in the ability to reduce
the noncommutativity errors incurred by
the
coning and sculling motion. Redesigning the dual
quaternion-based algorithm in the
frequency
domain would be a significant work. (In 2006, the
proposal obtained the support of
NSFC,
entitled “Dual Quaternion Strapdown Inertial
Navigation Algorithm in Frequency
Domain”,
60604011>
Within the dual-quaternion
mechanism, all navigation quantities (including
attitude, velocity
and position> can be
derived by manipulating the solutions to the three
kinematic equations.
This makes it possible to
model the error propagation completely in terms of
quaternion
algebra. Chapter 3 is devoted to
error characteristics of the strapdown inertial
navigation using
dual quaternion. Two new
error models in terms of quaternion algebra are
developed: the
additive dual quaternion error
model and multiplicative dual quaternion error
model. Both are
expected to facilitate the
future dual quaternion-based integrated navigation
filter. (Published
in IEEE Trans.
on
Aerospace and Electronic Systems, 2006>
Dual
quaternion navigation algorithm directly outputs
navigation parameters in the Earth
frame, but
in applications such as GPS navigation and
geodesy, we often need them expressed
in the
local level frame. Chapter 4 investigates a sub-
problem involved in the dual quaternion
navigation algorithm, i.e., the transformation
from Earth-centered Earth-fixed coordinates to
geodetic coordinates. We come up with an
iterative approach using the Newton-Raphson
method that has good efficiency and accuracy
and is free from singularity and divergence
except in a small region near the center of
the Earth. Comparisons with existing methods show
the new algorithm has much higher accuracy and
lower arithmetic complexity. (Published in
of
Earth-centered Earth-fixed coordinates to geodetic
coordinates,IEEE Trans.
on Aerospace and
Electronic Systems, 2003>
4 7
As a self-contained dead reckoning method,
inertial navigation has an inherent limitation
with its
error accumulating as time goes. So
inertial navigation must be integrated with
extraneous
information feedback to form a
stable and closed-form integrated navigation
system, so as to limit
the error accumulation.
Many kinds of integrated schemes emerge as the
times require. Integrated
navigation is aimed
to improve the system accuracy and redundancy
using compensatory
information from various
sensors. Whichever configuration (inertial
navigation, satellite navigation,
radar,
camera, Doppler speedometer and altimeter, etc.>
the integration takes, we must rely on an
optimal on-line information fusing strategy,
i.e., filtering, to efficientlyintegrate various
information
sources. Therefore, nonlinear
filtering is the cornerstone of any integrated
navigation system. In
fact, the well-known EKF
is one direct outcome of integrated navigation
system requirements.
Since its first
application in Apollo Project in 1960s, EKF has
prevailed for half a century and
actually
becomes a standard component in engineering. On
the other hand, EKF is a kind of
approximate
Gaussian filter and experiences has indicated its
limitations, e.g., linearization of any
considered nonlinear system and being apt to
divergence. In recent decades, nonlinear filtering
researches have made many achievements and
advanced nonlinear filters are being considered as
a
feasible information fusion strategy for
integrated navigation.
Then, could we
find or design general nonlinear filters to
replace EKF? From the aspect of
information
theory, it belongs to a problem of searching real
information fusion strategies,
i.e.,
filtering, superior to EKF.
Specifically,
the latter part of the thesis has been focused on
nonlinear Gaussian filtering.
4. Bayesian
inference provides an optimal solution framework
for dynamic state estimation
problems. Because
the Bayesian solution requires the propagation of
the full probability
density, in general the
optimal nonlinear filtering is analytically
intractable. Approximations
are thus
necessary, e.g., Gaussian approximation to the
posterior probability density. The class
of
filters derived under Gaussian assumption is
commonly called as the Gaussian filters. So
far, there has been a variation of Gaussian
filters that derived themselves from very
different
backgrounds. A question now arises:
with so many different Gaussian filters, how to
decide
which one is suitable for a filtering
problem in hand? Chapter 4 reviews the state of
art of
Gaussian filters from the perspective
of numerical integration. Specifically, we present
in a
unified numerical-integration framework
the derivation of a number of approximate Gaussian
filters. It shows that all Gaussian filters
are approximations of the general Gaussian filter
by
using a specific numerical integration
method of some kind or another, such as the
Gauss-
Hermite product rule, rules exact for
monomials and methods of approximation. This
perspective provides a well-founded
understanding of all the existing Gaussian filters
with
respect to accuracy, efficiency and
stability factor. The analytical findings are
tabulated, from
which a ranking of accuracy of
various Gaussian filters is derived. The numerical
results agree
nicely with the analytical
ranking list. We believe that this perspective has
set a good
5 7
5.
6.
foundation for selection of Gaussian filters
in practice and hopefully be useful to design more
efficient and stable filters by employing
better numerical integration methods. (Published
in
on particle filteringIEEE Trans. on Signal
Processing, 2005；
numerical-integration
perspective on Gaussian filters,IEEE Trans. on
Signal Processing,
2006>
Chapter 6
analyzes and compares two alternative versions of
unscented transformation (UT>-
based filters
for the nonlinear dynamic system with additive
noises: the non-augmented
unscented Kalman
filter (UKF> and the augmented UKF. They can be
both applied to
nonlinear dynamic systems with
additive noises. It is now believed that for the
special (but
often found> case where process
and measurement noises are additive, the
computational
complexity can be reduced by
using the non-augmented form, which presumably
yields
similar results, if not the same. In
this chapterwe will show that this assumption is
not quite
correctand that the non-augmented
UKF usage can lead to noticeable losses in
accuracy.
Firstly, it is proved that the non-
augmented UT is identical to the augmented
counterpart only
if certain condition is
satisfied. We point out that the basic difference
between the augmented
and non-augmented UKFs
is that the former draws sigma points only once in
a recursion while
the latter has to redraw a
new set of sigma points to incorporate the effect
of additive process
noise. This difference
generally favors the augmented UKF in that the
odd-order moment
information is captured by
the transformed sigma points and well propagated
within one
recursion. On the other hand, if a
new (but unnecessary> set of sigma points were
redrawn in
the augmented UKF, it would be
identical to and yield exactly the same results as
the non-
augmented UKF. The simulation results
of a representative example agree well with our
conclusions. (Published in Kalman filtering
for additive noise case: augmented
versus non-
augmented,IEEE Signal Processing Letters, 2005。 on
“Performance evaluation of UKF-based nonlinear
filtering”,Automatica, 2007>
Gaussian particle
filter (GPF> is a kind of Gaussian filter based on
Bayesian
actually extends the conventional
Gaussian filter using Monte Carlo integration and
the
Bayesian update r 7 further extends GPF
from two different aspects. Firstly, a
so-
called quasi-Gaussian particle filter
(QuasiGPF> is proposed that generalizes the GPF by
relaxing the Gaussian restriction on the prior
probability density function. Considering the
non-Gaussianity of the prior probability, the
QuasiGPF is provably superior to the GPF.
Numerical results show its remarkably improved
performance over the GPF. Theoretically, the
posterior probability could be assumed to be
any other distribution as long as it was readily
sampled, e.g., mixed Gaussian. In such a case,
it is promising for the QuasiGPF to be used to
construct more superior filter than the GPF-
based Gaussian sum particle filter. Secondly, the
low discrepancy sequences are invented to
distribute deterministic correlated draws over the
target domain as uniformly as possible. The
quasi-Monte Carlo method using these sequences
can be used to approximate multidimensional
integrals and further to sequential Bayesian
statistical inference. We investigate a
special version of Bayesian filtering, i.e., the
GPF, via
quasi-Monte Carlo method. Numerical
results show that the new GPF outperforms the
conventional GPF using random numbers in the
sense of having lower MSE and faster
6 7
convergence.
Keywords: inertial
navigation, dual quaternion, numerical
integration, error
characteristics, coordinate
transformation, nonlinear Gaussian filtering,
particle
filtering, low-discrepancy sequence
7 7