包围表示由非线性模型控制的至少一个移动目标的运动的状态的受限制全对称多胞形的估计方法与流程

1.本发明涉及集员(set-membership)状态估计技术的技术领域。
2.本发明处理包含状态的受限制全对称多胞形(constrained zonotope)的估计方法，所述状态表示由非线性模型控制的至少一个移动目标的运动。


背景技术：

3.在过去几年中，无人驾驶运载工具的自主水平有了相当大的提高，使得目前正在努力将无人驾驶飞机系统(unmanned aircraft systems，uas)整合到国家领空中，并将自动驾驶汽车整合到通常的交通中。长时间以来，已知协作运动策略可以带来许多具有巨大经济潜力的益处。地面运载工具的自动队列行驶可以增加公路通行能力，避免交通拥堵，减少对单独运载工具的耗用。紧密编队飞行已被证明可以显著提高飞机的航程。最突出的实例包含航程改进，这要归功于在紧密飞机编队中利用了尾涡(wake vortex)效应和自动空中加油。为了以安全且有效的方式执行这些协作运动策略，需要以分米级甚至厘米级准确度来估计运载工具的相对位置。
4.对于紧密编队飞行，由于基于全球导航卫星系统(global navigation satellite system，gnss)的独立定位无法提供所需的准确度，因此过去提出了各种差分定位技术。在过去几十年的文献中可以找到许多传感器融合方法，其采用惯性测量单元，利用或不利用相位信息、机器视觉或两者的组合的差分gnss。最近，已成功展示了便宜、分米精度的超宽带(ultra-wideband，uwb)测距传感器可提高机载实时运动学(real time kinematics，rtk)解决方案的稳健性。
5.这些现有方法的共同点是它们依赖于概率滤波技术，如无迹卡尔曼滤波器(unscented kalman filter，ukf)或扩展卡尔曼滤波器(extended kalman filter，ekf)。众所周知，许多实际的非线性估计问题违反了ukf和ekf两者的基本假设，特别是已知的过程模型和观测值模型不确定性分布的关键假设。实际上，通常仅可获得两种不确定性来源的随机特性的逼近。尽管存在这个固有且众所周知的问题，但ekf和ukf已被证明在实践中运行良好，提供了准确的平均状态估计。
6.然而，当涉及到多个极为接近的智能体的安全协作运动时，滤波器状态不确定性的一致计算比准确的点估计更为重要。由于所涉及的逼近(由ekf和ukf的无迹变换执行的线性化步骤)，在采用ekf或ukf时，保证滤波器一致性是非线性估计问题的一个开放问题。然而，我们认为可以计算运载工具状态的保证范围的算法是将自动驾驶运载工具进一步安全融入到异构环境(诸如混合的空中交通和普通道路交通)中的关键要素。
7.在这点上，集员(set-membership)状态估计技术为概率框架提供了一个令人关注的替代方案。在假设观测值和过程模型不确定性包含于已知的紧集(compact sets)(例如，区间)中的情况下，集员估算器依赖于集值过程模型。不需要对这些集合中的误差分布进行假设。对于非线性系统，准确的集员估计问题通常在计算上是难以处理的。研究旨在寻找所
涉及的集合运算的可实施但紧密的外逼近。在过去几十年中，已经提出了许多算法，其采用如椭圆形或区间的集合形状。所有集员估算器面临的共同挑战包括在保守性(通过用更简单的、计算上易于处理的集合表示复杂集合而引入的过逼近度)和计算工作量之间找到折衷。
8.尽管集员方法有其优点，但它们在相关智能体定位中的应用却很少受到关注。已提出一种基于区间分析的集反演(set inversion via interval analysis，sivia)算法的地面运载工具协作集员定位算法。sivia和相关非线性集反演算法在应用于状态估计算法时提供了紧密的范围，其逼近度仅受限于可用的计算资源。然而，这些算法的复杂度通常会随着状态数量的增加而快速增加，从而限制了其在2d地面运载工具定位应用之外的用途。
9.归功于其恒定的复杂度，椭圆形提供了良好的计算性能，并已成功应用于模拟中的相关智能体定位。最近，全对称多胞形在保证的状态估计方面受到越来越多的关注。全对称多胞形提供比椭圆形更多的灵活性，其复杂度为设计参数。全对称多胞形的一个固有问题为其对称性，其在实际状态集展现出强烈的不对称性时会引入额外的保守性。第二个问题为两个全对称多胞形的交集并不总是产生全对称多胞形，因此，如椭圆形的情况，必须在每次滤波器递归时计算中间集的外逼近，这可能涉及求解凸优化问题。


技术实现要素：

10.本发明旨在解决或至少缓解这两点不足。
11.本发明的第一方面为如技术方案1所述的方法。
12.在如技术方案1所述的方法中使用的先验全对称多胞形为受限制的全对称多胞形。此类全对称多胞形允许更好地逼近不对称的集合形状。作为额外优点，其在相交处和闵可夫斯基(minkowski)和运算下是封闭的。
13.根据本发明的第一方面的方法可以包括以下任选特征。
14.非线性转换模型被线性化的元素可以为包含第一受限制全对称多胞形的最小区间范围的中心。
15.所述方法可以进一步包括在第一受限制全对称多胞形或先验全对称多胞形的元素处线性化非线性观测值模型，以产生线性观测值模型，其中在步骤e)所使用的观测值模型为所产生的线性观测值模型。
16.非线性观测值模型被线性化的元素可以为包含先验全对称多胞形的最小区间范围的中心。
17.所述方法可以进一步包括：
18.·
计算在第一受限制全对称多胞形或先验全对称多胞形的不同元素处的观测值线性化误差，其中在第一受限制全对称多胞形或先验全对称多胞形的给定元素处的观测值线性化误差是将非线性观测值模型应用于给定元素所得的输出与将线性观测值模型应用于给定元素所得的输出之间的差，
19.·
计算包含所有计算的观测值线性化误差的第四受限制全对称多胞形，其中后验受限制全对称多胞形取决于第四受限制全对称多胞形。
20.移动目标的观测值数据可以在不同于第二时刻的第三时刻处获取，并且所述方法可以进一步包括：
21.·
使第四受限制全对称多胞形扩大，以获得扩大的受限制全对称多胞形，其在状态空间中形成条带并考虑到在第二时刻与第三时刻之间的运动状态的所有可能变化，并且可能地，考虑到由传感器引起的测量值噪声，
22.·
将后验受限制全对称多胞形计算为先验受限制全对称多胞形与扩大的受限制全对称多胞形之间的交集。
23.所述方法可以进一步包括：
24.·
确定后验受限制全对称多胞形的复杂度水平，
25.·
如果复杂度水平大于阈值，则将归约过程应用于后验受限制全对称多胞形，以获得复杂度比归约前更低且包含后验全对称多胞形的后验全对称多胞形。
26.所述方法可以进一步包括：在随后执行技术方案1中定义的步骤a)到e)时，将后验受限制全对称多胞形用作第一受限制全对称多胞形并且将第二时刻用作第一时刻。
27.所述方法可以进一步包括：
28.·
计算第一时刻与传感器获取观测值数据的时刻之间的最大时间区间，
29.·
如果最大时间区间大于预定持续时间，则将线性转换模型应用于预定的受限制全对称多胞形，以产生先验受限制全对称多胞形，否则将预定的受限制全对称多胞形用作先验受限制全对称多胞形。
30.观测值数据可以包括由gnss接收器获取的以下数据中的至少一个：伪距、获取的载波相位、移动目标与另一移动目标之间的距离。
31.运动数据可以包括移动目标相对于另一移动目标的运动数据。
32.可以利用借助区间分析的向量集反演算法来计算第一受限制全对称多胞形。
33.本发明的第二方面为一种包括指令的计算机程序产品，在由计算机执行程序时，所述指令使得所述计算机执行本发明的第一方面的方法。
附图说明
34.图1示意性地表示用于估计包围移动目标的运动状态的集合的装置；
35.图2说明用于估计包围系统的状态x的c-全对称多胞形的示例性方法的步骤；
36.图3说明图2中示出的方法的传播步骤的操作序列；
37.图4为目标装置和信标的位置p和到x轴的距离d的图形表示；
38.图5表示图2中示出的方法的传播、更新和归约步骤的快照；
39.图6为目标装置的真实状态轨迹的图形表示；
40.图7为目标装置的czesmf范围的图形表示；
41.图8为目标装置的位置区间范围的图形表示；
42.图9为目标装置的速度区间范围的图形表示；
43.图10为另一目标装置的位置的图形表示；
44.图11表示通过vsivia获得的子铺设体(subpaving)的区间范围；
45.图12表示针对图10的目标装置的所获得先验c-全对称多胞形连同所获得后验c-全对称多胞形和归约的后验c-全对称多胞形；
46.图13表示在观测值定时噪声增加的情况下的先验c-全对称多胞形和后验c-全对称多胞形；
47.图14表示图10的目标装置的全状态范围轨迹；
48.图15表示在观测值定时噪声增加的情况下图10的目标装置的全状态范围轨迹；
49.图16为通过两个不同方法获得的距离线性化误差区间测量的两个半径的图形表示；
50.图17表示在模拟期间其它两个目标装置的位置；
51.图18表示在利用gnss更新或gnss+uwb更新的情况下通过速度范数预测和通过t3d预测获得的集合体积；
52.图19表示利用czesmf滤波器和利用iesmf滤波器获得的相对位置集合体积。
具体实施方式
53.以下描述的结构如下：在定义符号(第1部分)并回顾集员状态估计问题(第2部分)之后，将描述一种基于受限制全对称多胞形(c-全对称多胞形)的计算上有效的状态集估计方法(第3部分)。接着，将给出所述方法的三个实际应用作为实例：
54.·
简单的1d示例(第4部分)，
55.·
稍微更复杂的2d定位问题(第5部分)，
56.·
利用差分gnss和对等测距观测值的3d对等定位基准问题(第6部分)。
57.1.符号
58.为了适应这种复杂度，下文将使用连续时间估计理论中熟悉的符号x(t
l
)。滤波器执行的离散特性是通过对时间进行索引来捕获的。具体来说，tk指定由处理单元计算先验和后验状态集的时间。对于向量x(t)，时间t表示该向量的有效时间。
59.相应地，集合z(tk)在时间tk是有效的，即如果t＝tk，则x(t)∈z(tk)。如果有效时间是区间[tk]，则所述集合对于在此区间内的所有时间都有效，即如果t∈[tk]，则x(t)∈z(tk)。
[0060]
此外，矩阵j
nxm
表示所有元素为一的n行m列矩阵。大小为n的单位矩阵由in表示，大小为n的零方阵由0n表示。
[0061]
两个向量a、b的点积表示为《a,b》。
[0062]
区间矩阵由表示，其中m、的元素分别是[m]的区间元素的实数下限和上限。对于m＝1且n＞1，我们称其为区间向量，即[p]。标量区间由表示，其中下限和上限x、对于区间[m]、m表示区间的实数化，即m∈[m]。
[0063]
在假设附加误差的情况下，将区间矩阵以中心范围符号写为[m]＝mid(m)+[-rad(m),rad(m)]会更方便，其中mid(m)是[m]的中心点且区间半径矩阵由给出。当项和区间项出现在同一个表达式中时，如[c]＝a+[b]，则假设项(a)表示退化区间([a,a])，并且表达式中的所有运算都是区间运算。
[0064]
由于c-全对称多胞形在非线性变换下不是封闭的，因此在下面使用非线性系统等式的一阶泰勒逼近，参见第3.2部分和第3.3部分。为了确保这些逼近不会损害所得状态范围的一致性，使用了线性包含的概念，其它集合估计方法也是如此，参见例如e
·
斯科尔特等人的“用于在线应用的非线性集员滤波器(a nonlinear set-membership filter for online applications)”。非线性函数的线性包含是一个集值线性函数，它保证包含非线性
函数在某个输入集上的解。作为一示例，考虑函数
[0065]
y＝h(x)
ꢀꢀꢀ
(1)
[0066]
其中，x∈x，即x包含在某个紧集x中。线性函数
[0067][0068]
为等式1的线性包含，条件为
[0069]
y∈y0+hx+[∈]
ꢀꢀꢀ
(3)
[0070]
对于所有x∈x，即线性集值函数的集值解总是包含非线性解。
[0071]
2.问题陈述
[0072]
可以通过包括运动数据的运动状态来描述移动目标的运动。所述运动数据可以包括例如：目标的位置、目标的速度、目标的加速度、其它数据或其组合。当移动目标移动时，运动状态会随着时间而变化。
[0073]
在下文中，为简单起见，移动目标1可以称为“系统”，并且运动状态可以称为“状态”。
[0074]
参考图1，用于估计包围移动目标的运动状态的集合的装置1包括处理单元2和观测单元4。
[0075]
处理单元2配置为执行包括实施下文将描述的估计方法的代码指令的计算机程序。该计算机程序将在下文中被称为“czesmf滤波器”(受限制全对称多胞形的扩展集员滤波器)或“滤波器”，因为其实际上就像一个滤波器。
[0076]
所述滤波器配置为利用预定的转换模型(下文中还称为“传播模型”或“动态模型”)和预定的观测值模型。
[0077]
所述处理单元2配置为在不同处理时刻处操作，使得能够估计在所述不同时刻处包围移动目标的运动状态的不同集合。
[0078]
所述处理单元2可以为以下任何类型：一个或更多个处理器、一个或更多个微处理器、fpga、asic等。
[0079]
所述观测单元4包括配置为观测移动目标的至少一个传感器。因此，观测单元4配置为输出与移动目标相关的观测值数据。所述观测单元还配置为在不同观测时刻操作，使得能够在所述不同时刻处获取不同观测值。
[0080]
集员状态估计的目的是计算区域其中系统的状态x(t)保证驻存于时间t处。
[0081]
2.1.状态动态
[0082]
在离散状态估计中，通常假设系统状态动态由以下类型的静态模型控制
[0083]
x(tk)＝f(x(t
k-1
),u(t
k-1
))+q(t
k-1
)
ꢀꢀꢀ
(4)
[0084]
所述模型具有先前状态系统输入传播模型误差以及恒定采样时间ts＝t
k-t
k-1
。
[0085]
在实践中，不规则的观测值时间产生不规则的采样时间，即滤波器中的传播视距。根据采样时间，不同传播模型可以产生具有不同紧密度的状态范围。一个来自导航的实际示例是惯性航位推算定位(inertial dead-reckoning localization)，其中观测到的加速度的二重积分产生非常紧密的短期位置范围，但传感器偏差导致位置范围的长期二次增
长。相比之下，运载工具动态的简单速度范数界限模型(示例参见第5部分)使得位置范围随时间线性增长，因此可能为更长的传播视距提供更紧密的范围。
[0086]
在第6部分中处理另一实例，其中利用时间差分gnss相位观测值的紧密相对位置传播仅在gnss接收器的采样时间可用，而在gnss观测值之间，只有粗略的速度范数界限传播模型可用。
[0087]
我们通过假设非常一般的时变状态动态模型来适应多个传播模型，所述动态模型为非线性映射
[0088]
x(tk)＝f(x(t
k-1
),u(t
k-1
),t
k-1
,tk)+q(t
k-1
)
ꢀꢀꢀ
(5)
[0089]
自变量tk和t
k-1
表示模型在从t
k-1
到tk的时间范围内执行系统状态的传播以及f()的时变特性。归功于允许传播模型为时变的，滤波器可以在每个传播步骤102处(参见图2)采用一组可用模型中最不保守的模型。
[0090]
2.2.观测值
[0091]
在实践中，由观测单元4进行的观测通常既不是完全同步的也不是完全周期性的。更复杂的是，观测值时间可仅为大致已知的，例如由于缺乏硬实时通信协议。
[0092]
观测值时间不确定性导致现有集员状态估计方案尚未解决的一个细微但严重的问题。传播状态范围保证仅在选定的传播时间tk处包含状态。我们称其为给定状态范围的有效时间。由于观测值对应于一区间内的某未知时间，因此利用此观测值来更新先验范围会导致在任何时间都不能保证状态范围有效，即使tk在观测值时间区间内也如此。这仅仅是因为和对应于观测值的状态空间限制不涉及同一时间点。因此，忽略观测值时间不确定性在仅一次更新后导致无效的状态范围。下文呈现针对此问题的补救。
[0093]
假设观测值由非线性映射控制
[0094][0095][0096]
其中，是传感器观测值的向量，w(t)∈w是相应的测量误差，是观测到的观测值时间戳，w
t
(t)∈[w
t
]是观测值时间误差，且t是真实观测值时间。观测值时间戳误差w
t
(t)不仅涵盖定时抖动，还涵盖例如由于在数字数据链路上的数据包编码/解码延迟而产生的通信延迟。所有时间都为挂钟时间。
[0097]
应注意，除了包含在某已知集合w(t)中的测量模型误差之外，没有对测量模型误差做任何假设。照此，它涵盖硬件噪声、偏差和建模误差。还应注意，观测值模型h(...)和观测值向量的大小m(t)两者都可以为时变的。
[0098]
虽然y(t)表示由模型提供的观测值，但从传感器接收到的实际测量值由z(t)表示。
[0099]
3.基于c-全对称多胞形的扩展集员滤波器
[0100]
在集合状态估计中使用的最流行的计算上有效的集合表示是基本几何实体的线
性变换，如椭球-变换的单位超球和全对称多胞形-变换的单位超立方体。超球和超立方体的固有对称性限制了椭球和全对称多胞形紧密逼近不对称或通常形状不规则的集合的能力。使情况进一步恶化的是，在滤波器更新的交集操作下，两者都没有封闭。因此，需要计算每个交集结果的外逼近。受限制全对称多胞形缓解了这两个问题。其为受限制的单位超立方体的线性变换，照此可为不对称的。其在交集下是封闭的，即两个c-全对称多胞形的交集是另一个c-全对称多胞形，但其复杂度更高。
[0101]
c-全对称多胞形由其生成矩阵g、其中心c和单位超立方体的约束参数a,b来定义，并描述了集合：
[0102]
z＝{gξ+c|‖ξ‖
∞
≤1,aξ＝b}
ꢀꢀꢀ
(8)
[0103]
每个约束表示与单位超立方体相交的平面。然后，所得交集通过生成矩阵g进行投影，最后通过中心向量c进行平移。
[0104]
c-全对称多胞形是凸多胞体的替代参数化。此外，c-全对称多胞形是规则全对称多胞形的超集，因为每个没有约束的c-全对称多胞形都是一个规则全对称多胞形。为简洁起见并根据现有文献，凸多胞体的c-全对称多胞形表示标记为g-rep.且其半空间表示标记为h-rep。
[0105]
c-全对称多胞形z的区间范围由[z]标记，区间[a]的g-rep.由z([a])标记。
[0106]
参考图2，用于估计包围系统状态x的c-全对称多胞形的方法包括由滤波器基于在时间t
k-1
处包围状态的预定c-全对称多胞形z
x
(t
k-1
)执行的以下主要步骤：
[0107]
·
传播步骤102，其中处理预定c-全对称多胞形以获得在时间tk处包围状态的先验c-全对称多胞形
[0108]
·
更新步骤104，其中先验受限制全对称多胞形与由观测单元4获取的观测值数据组合，以获得在时间tk处包围状态的后验c-全对称多胞形z’x
(tk)。
[0109]
在时间tk处包围状态x并考虑直到且包含tk的所有观测值的后验c-全对称多胞形标记为
[0110]z′
x
(tk)＝{g
′
x
(tk),c
′
x
(tk),a
′
x
(tk),b
′
x
(tk)}
ꢀꢀꢀ
(9)
[0111]
而在执行更新之前，在时间tk处包围状态x的先验c-全对称多胞形标记为
[0112][0113]
下面将详述传播步骤102和更新步骤104可以分别使用线性转换模型和线性观测值模型，所述线性模型是通过在特定点线性化非线性模型而获得的。
[0114]
此外，所述方法可以进一步包括归约步骤106，其中滤波器将后验c-全对称多胞形z’x
(tk)转化成另一个复杂度较低但包含c-全对称多胞形z’x
(tk)的c-全对称多胞形z
x
(tk)。
[0115]
传播步骤102、更新步骤104和归约步骤106递归地重复。更准确地说，所述方法包括多个递归，每个递归执行传播步骤102、更新步骤104和归约步骤106。
[0116]
所述方法的第一次递归之前是初始化步骤100，在所述步骤中确定初始c-全对称多胞形
[0117]
现将详细地描述步骤100、102、104和106。
[0118]
3.1.初始化
[0119]
已知初始状态包含在某些c-全对称多胞形中，即第一先验状态范围中。
[0120]
3.2.传播
[0121]
在传播步骤102中，对于k＞1执行，滤波器计算先验状态集在给定前述后验状态集z
x
(t
k-1
)的情况下，保证在时间tk＞t
k-1
处包含状态。
[0122]
由于不确定观测值的特殊性，出现了如何选择计算先验c-全对称多胞形的时间的问题。甚至可以在将来滤波器递归开始时，以补偿滤波器执行时间。然而，根据递归(传播和更新)的执行时间，将tk设置为接近观测值时间并执行最终传播以补偿执行时间可能更有意义。
[0123]
一些选项是标称的周期性观测值时间(如果观测值是标称周期性的)或观测值时间区间的中心。由于第一选项的有益效果，滤波器结果变成周期性的，从而在滤波器中隐藏了观测值时间的不确定性。
[0124]
由于c-全对称多胞形在非线性映射下不是封闭的，类似于扩展卡尔曼滤波器(extended kalman filter，ekf)，我们通过形成关于操作点的状态动态模型等式(5)的一阶泰勒逼近开始。选择前述后验状态的区间范围和输入范围的中心作为操作点：
[0125][0126]
其中zu＝{gu,cu,au,bu}
ꢀꢀꢀ
(12)
[0127]
由于c-全对称多胞形中心甚至不一定位于其c-全对称多胞形内，因此这个选项往往会导致更小的线性化误差，但不会最小化线性化误差。
[0128]
线性化将我们引向熟悉的一阶泰勒展开式
[0129][0130]
其中，线性化误差为∈f(tk)∈[∈f(tk)]，并且其中
[0131][0132][0133]
相应的线性包含由下式给出
[0134]
x(tk)∈f(t
k-1
)z
x
(t
k-1
)+b(t
k-1
)zu(t
k-1
)+zq(t
k-1
)+z([εf(t
k-1
)])+z([cf(t
k-1
)]
ꢀꢀꢀ
(17)
[0135]
然后，从时间t
k-1
到时间tk的传播执行为
[0136][0137]
等式18中的矩阵乘法和求和运算是对由j
·k·
斯科特等人在“受限制全对称多胞形：一种基于集合的估计和故障检测新工具(constrained zonotopes:a new tool for set-based estimation and fault detection)”中定义的c-全对称多胞形的集合运算。zq(t
k-1
)包围非线性状态动态模型的线性化误差和模型不确定性，且zu(t
k-1
)是任何不确定输
入的c-全对称多胞形范围。传播不确定性项q(tk)实际上对应于ekf的过程噪声，而c-全对称多胞形zq(tk)对应于ekf的过程噪声协方差矩阵q。
[0138]
[∈f(tk)]可以看作是在c-全对称多胞形z
x
(t
k-1
)的不同元素处的一组转换线性化误差。在z
x
(t
k-1
)的给定元素处的转换线性化误差是将非线性转换模型应用于给定元素所得的输出与将线性转换模型应用于给定元素所得的输出之间的差。
[0139]
此外，上述表达式zq(t
k-1
)+z([εf(t
k-1
)])+z([cf(t
k-1
)]可以看作包含所有线性化误差的c-全对称多胞形。此外，表达式可以看作是将线性转换模型应用于z
x
(t
k-1
)所得的c-全对称多胞形。后验全对称多胞形是两个表达式的和。
[0140]
3.2.1.条件传播
[0141]
在递归滤波方案中，传播步骤102的目的是获得状态范围，所述状态范围在对系统状态进行观测时是有效的，或在接近对于系统状态进行观测时的情况下是有效的。
[0142]
在本方法中，有两种情况不需要执行传播，从而减少滤波器运行时间。
[0143]
第一种情况发生在对一系列同步向量观测值执行更新104时。在这种情况下，利用第一向量元素的更新104所得的后验c-全对称多胞形可以被重新用作先验c-全对称多胞形，用于随后利用观测值向量的其余元素进行顺序更新。
[0144]
此外，当观测值在时间上接近时，并非执行传播，为了进一步扩大观测值区间(参见第3.3.1部分)，将观测的有效时间延长，使得超过前述后验c-全对称多胞形的有效时间可为能够接受的(就增加的保守性来说)。为了适应这种情况，滤波器使用阈值在每次传播之前，滤波器确定是否需要执行传播。
[0145]
下面的算法1说明如何在实践中能够实施条件传播(conditional propagation)：(也参见图3)：
[0146][0147]
3.3.更新
[0148]
为了执行更新，非线性观测值等式被线性化以形成其线性包含。通过区间算术(interval arithmetic，ia)来评估余数(即线性化误差)，可以保证其在先验状态范围上有效。然后，观测值向量的每一元素定义了状态空间中的条带。先验c-全对称多胞形可以同时或顺序地与所有这些条带相交。如椭球和全对称多胞形的流行集合形式在与条带相交处不是封闭的，照此需要每个交集结果的外逼近，这使得顺序交集不是最优的。另一方面，利用顺序更新，由于线性化误差区间收紧，在每个交集之后重新线性化观测值等式可能会导致明显更紧密的更新。这两种相反效应的结果取决于所涉及的观测值的非线性程度，并且照此是应用相关的。
[0149]
作为c-全对称多胞形的独特特征，c-全对称多胞形在与条带相交处是封闭的。因此，同时或顺序地利用每个标量元素更新整个观测值向量是等效的操作。因此，在利用c-全对称多胞形的情况下，我们可以从以下两种效果中受益：顺序更新的简单性而不损失交集最优性，和重新线性化的收紧效果。
[0150]
因此，在不损失一般性的情况下，在下文中考虑标量观测值。在运行滤波器递归之前，同步观测值的向量被转化为一系列具有相同时间戳的标量观测值。因此，在下文中，将较简单的标量观测值等式用作示例：
[0151][0152]
其中，t
y,k
是观测值k的真实观测值时间，且是受定时误差影响的视在(apparent)观测值时间。
[0153]
在假设区间值误差的情况下，在时间t
y,k
处有效的非线性观测值映射的一阶逼近的形式为
[0154][0155]
其中
[0156][0157][0158][0159][0160]
其中，∈h(tk)＝∈
h,x
(tk)+∈
h,u
(tk)∈[∈h(tk)]是先验状态和输入zu上的线性化误差。应注意，tk是先验c-全对称多胞形的有效时间。相应的线性包含则由下式给出
[0161]
y(tk)∈h(tk)z
x
(tk)+b(t
k-1
)zu(tk)+zq(tk)+z([εh(tk)])+z([ch(tk)]
ꢀꢀꢀ
(26)
[0162]
实际上，这里计算了线性观测值等式和包含此线性等式对于所有先验c-全对称多胞形状态的误差的集合。此误差具有多个分量：
[0163]
·
d(tk)zu(tk)：系统输入的影响，例如由自动驾驶仪或外部扰动产生的控制输入。c-全对称多胞形zu包含所有可能的输入或扰动。
[0164]
·
z[∈h(tk)]：转化成c-全对称多胞形的线性化误差区间。这个全对称多胞形包含在先验全对称多胞形的不同元素处的所有观测值误差。在先验全对称多胞形的给定元素处的观测值线性化误差是将非线性观测值模型应用于给定元素所得的输出与将线性观测值模型应用于给定元素所得的输出之间的差。
[0165]
·
z([ch(tk)])：表示线性化观测值等式的所有常数项和原始非线性观测值等式的测量值噪声区间的c-全对称多胞形。
[0166]
为了执行所述更新，将两个集合相交。第一个集合由先验c-全对称多胞形给出(等式18)。第二个集合(参见等式35)由观测值定义。由于我们知道观测值z(t
y,k
)包含在等式26的右侧，因此我们可以在等式26中将y(t
y,k
)替换为z(t
y,k
)，以形成等式35。接着，对于标量观测值，等式35表示状态空间中的条带，参见图3。显然，第二个集合不是c-全对称多胞形，因为它不是封闭的(条带无限延伸)。但是，其与先验c-全对称多胞形的交集是c-全对称多胞形(后验c-全对称多胞形)。
[0167]
应注意，虽然线性化系统等式为逼近步骤，但相应的线性包含保证包括相应的非线性解。
[0168]
3.3.1.观测值区间扩大
[0169]
回顾到，已知真实的观测值时间具有有界的不确定性，即已知其在区间[t
y,k
]内。换句话说，观测值在这个时间区间内的某处是有效的。由于不能保证观测值的有效时间与先验c-全对称多胞形一致，因此可能无法执行更新。
[0170]
作为补救这种情况的简单方法，扩大观测值误差区间从而延长观测值的有效时间。从线性包含等式26开始，先验c-全对称多胞形的有效时间与观测值之间的时间由以下区间来限界
[0171]
[δtk]＝t
k-[t
y,k
]
ꢀꢀꢀ
(27)
[0172]
其中，状态增量区间
[0173]
[δx(tk)]
ꢀꢀꢀ
(28)
[0174]
满足
[0175][0176]
如果
[0177]
t∈tk+[t
y,k
]
ꢀꢀꢀ
(30)
[0178]
我们可以计算扩展的线性包含
[0179]
y(t
y,k
)∈h(tk)z
x
(tk)+d(tk)zu(tk)+zq(tk)+z([εh(tk)])+z([ch(tk)]+z(h(tk)[δx])
ꢀꢀꢀ
(31)
[0180]
扩大步骤的主要目标是解决以下问题：观测值的有效时间不能保证与先验c-全对称多胞形的有效时间一致。解决它的基本思路是扩大观测值误差区间，以确保其有效时间包括先验c-全对称多胞形的有效时间。这是通过计算观测值时间与的有效时间(等式27)之间的状态变化(等式28)的上限获得的。接着，所有已知项被一起分组到一个扩大的观测值误差区间中，从而产生等式32，其可由滤波器在更新步骤104中使用(等式36)。
[0181]
所有已知项都被归并到一个扩大的观测值误差区间中，其简化为
[0182]
y(t
y,k
)∈h(tk)z
x
(tk)+z([ε
′
])
ꢀꢀꢀ
(32)
[0183]
其中，([ε
′
])＝d(tk)[zu(tk)]+[εh(tk)]+[ch(tk)]+h(tk)[δx])
[0184]
应注意，现在需要在扩大的先验状态集上计算线性化误差∈h(tk)。为了实施，可以在区间范围上评估∈h(tk)，以便能够使用区间算术。由于c-全对称多胞形和运算中涉及的c-全对称多胞形的复杂度增加，这通常比选择替代区间更快。
[0185]
接着，对于标量观测值z(t
y,k
)，我们得到
[0186]
z(t
y,k
)∈h(tk)z
x
(tk)+z([ε
′
])
ꢀꢀꢀ
(33)
[0187]
且
[0188]
h(tk)z
x
(tk)∈z(t
y,k
)-z([ε
′
])
ꢀꢀꢀ
(34)
[0189]
∈z([ε])
ꢀꢀꢀ
(35)
[0190]
其中，[∈]＝z-[∈
′
]，并且其中，z([∈])＝{rad([∈]),mid([∈]),0,0}是区间[∈]的g-rep.。
[0191][0192]
其中
[0193][0194][0195][0196][0197]
有关传播和更新定时的概述参见图3。
[0198]
3.4.归约
[0199]
从等式36到41可以看出，每次更新都会增加状态c-全对称多胞形的复杂度-即生成器的数量ng和约束的数量nc。为了控制计算复杂度和内存占用，如果z
x
(tk)的复杂度超过由定义的某个选定的复杂度阈值，则滤波器在更新后执行归约。
[0200]
下面的算法2说明了如何在实践中能够实施归约步骤106：
[0201][0202]
下面的算法3、4说明根据实施例的由滤波器执行的不同步骤。
[0203]
[0204][0205]
4.1d应用
[0206]
在此第一示例中，我们考虑目标装置(或智能体)，其运动被限制在笛卡尔架构的x轴上，参见图4。我们仅基于对信标的距离观测值来估计其位置p和速度v。偏离x轴距离d定位的信标使观测值等式呈非线性，且照此需要非线性估算器，诸如czesmf。在此最小示例中，观测值时间不存在不确定性。
[0207]
状态x＝(pv)
t
的动态由以下等式控制
[0208][0209]
其中，ts＝t
k-t
k-1
且a(t)为加速度。
[0210]
距离观测值由以下等式控制
[0211][0212]
其中，wk∈[w]＝[-0.224,0.168]m。
[0213]
4.1.初始化
[0214]
我们假设最初目标的位置和速度被限于
[0215][0216]
4.2.传播
[0217]
在目标加速度由和ts＝t
k-t
k-1
限界的情况下，x(tk)的包含由下式给出
[0218][0219]
其中，zq(t
k-1
)＝qz([a])。
[0220]
由于包含等式46已经是线性的，因此我们直接获得如下的先验c-全对称多胞形
[0221][0222]
4.3.更新
[0223]
由于在此示例中观测值时间不具有不确定性，因此tk＝t
y，k
，即我们将状态范围传播到观测值的有效时间。从等式43形成如下线性包含
[0224]
r(tk)∈h(tk)z
x
(tk)+z([w])+z([∈h])+ch(tk)
ꢀꢀꢀ
(48)
[0225]
其中
[0226][0227]
且
[0228][0229]
在计算线性化误差的范围时，通过区间算术评估拉格朗日余数的一般方法通常会由于相依性而产生过于保守的范围。显式地评估∈h可以提供更紧密的范围，在利用单调性时更是如此。线性化误差的显式表达式为
[0230][0231]
为了检查单调性，考虑导数
[0232][0233]
为了在先验c-全对称多胞形的区间范围上计算∈h的紧密包含，我们只需要评估其由下式给出的顶点以及可能的拐点
[0234][0235][0236]
接着根据等式36执行更新。
[0237]
4.4.模拟
[0238]
图5中显示了传播步骤102、更新步骤104和归约步骤106的快照。图6和图7分别示出了真实状态轨迹和czesmf范围。图8和图9分别示出相应的位置和速度区间范围。在每次
更新之后，状态c-全对称多胞形已归约为
[0239]
5.2d定位应用
[0240]
在此示例中，我们考虑目标装置在具有两个静态测距信标的场地上移动(参见图10)。所述目标装置可能是一台自动割草机，其需要避免在指定区域内剪花，并避免离开所述场地的外围，或所述目标装置也可能是现代仓库中的自动叉车。
[0241]
目标的位置x＝(x
1 x2)
t
由以下等式控制：
[0242]
x(tk)＝x(t
k-1
)+q(t
k-1
)
ꢀꢀꢀ
(57)
[0243]
其中
[0244][0245]
其中，为所述目标的最大速度。如果无法更好地了解移动目标装置的内部动态，则此速度范数约束是一个保守但容易获得的动态模型。
[0246]
目标装置定期接收对位于不确定位置的两个信标的测距观测值
[0247][0248][0249]
其中
[0250][0251]
对每个信标ik的测距观测值由以下等式控制
[0252][0253]
这两个观测值标称地以1hz的频率同步。测距传感器与挂钟时间的不完全同步造成有界的定时抖动w
t
(t)∈[-10-2 10-2
]s。也就是说，一对测距观测值大约每隔一秒(但不是正好一秒)到达。
[0254]
为了获得周期性状态范围，在观测值时间区间的上限处触发滤波器递归，即(t1,t2,t3,t4,...)＝(0.01s,0.01s,1.01s,1.01s,...)。
[0255]
5.1.初始化
[0256]
目标装置最初在场地内。这允许计算对应于覆盖场地的区间的c-全对称多胞形。然而，这种极保守的初始范围导致了极保守的滤波器解。因此，利用vsivia来计算初始状态的更紧密范围。这个一次性步骤在二维上的计算成本不是很高(0.8s和233个处理框，其中∈＝0.1m)。对于更高状态维度的估计问题，最好将vsivia与约束传播联接起来，以在可接受的时间内计算足够紧密的初始状态范围。
[0257]
与通过vsivia获得的子铺设体的区间范围(在图11中表示为“解”和“未定义”)对应的c-全对称多胞形用于初始化滤波器。
[0258]
5.2.传播
[0259]
状态动态在状态中已经是线性的。因此，可以通过以下等式直接执行传播
[0260][0261]
由等式58定义的状态动态不确定性球由c-全对称多胞形(参见下文附录a)进行外逼近，以获得zs(tk-1)。所得先验c-全对称多胞形的示例显示在图12中。应注意，如何包含在t9处的真实位置。还应注意，t9对应于接近t＝4s执行的第一传播更新序列，因为两个测距观测值达到大约1hz，并且每个观测值都会增加滤波器递归索引k。
[0262]
5.3.更新
[0263]
其简化了在相应测距信标ik的标称帧中执行每次更新的过程。信标帧中的先验c-全对称多胞形由以下等式给出
[0264][0265]
接着，每一测距观测值由以下等式控制
[0266]
r(tk)＝||x(tk)-∈b||+w
r，k
ꢀꢀꢀ
(65)
[0267]
为了进一步简化，我们凭借以下等式将小信标不确定性吸收到观测值误差w
r，k
中
[0268]
||x||-||∈b||≥||x-∈b||≤||x||+||∈b||
ꢀꢀꢀ
(66)
[0269]
从而产生
[0270]
r(tk)＝||x(tk)||+w
′
r，k
ꢀꢀꢀ
(67)
[0271]
其中，w
′
r，k
∈[w
′
r，n
]＝[wr]+[-rad([∈b])，rad([∈b])]。
[0272]
从等式67我们形成如下线性包含
[0273]
r(tk)∈h(tk)z
x
(tk)+z([w
′r])+z([∈h])+ch(tk)
ꢀꢀꢀ
(68)
[0274]
其中
[0275][0276]
且
[0277][0278]
5.3.1.观测值区间扩大
[0279]
在补偿定时抖动之前，从速度范数界限获得状态增量区间[δx(tk)]
[0280][0281]
并且扩大的观测包含计算如下
[0282][0283]
在利用等式36到41计算信标帧中的后验c-全对称多胞形之后，所述后验c-全对称多胞形被平移回到导航框
[0284][0285]
图12示出在t9处获得的后验c-全对称多胞形，以及归约的后验c-全对称多胞形。图13示出在定时噪声值增加的情况下获得的结果。应注意，由于观测值区间扩大增加，所以
后验c-全对称多胞形更大。
[0286]
全状态范围轨迹显示在图14中，且在图15中显示了增加的观测值定时噪声。在每次更新之后，状态c-全对称多胞形已归约为通过将δt设置为0.1s，可以确保每对测距观测值仅执行传播一次，从而减少了滤波器运行时间(条件传播)。
[0287]
6.应用于3d对等定位
[0288]
在此示例中，所关注的状态是两个智能体(诸如紧密编队飞行的无人驾驶飞机)之间的相对位置。许多航空应用需要保证的相对定位，诸如空中加油、节能编队飞行和纳米卫星群。在此示例中，获得原始gnss码和载波相位观测值以及uwb对等测距观测值。两种传感器都便宜，质量非常低-几十克-而且易于作为商用现货(commercial-off-the-shelf，cots)组件随时提供。我们展示了如何将czesmf应用于这个具有高度实际相关性的定位问题，并与现有的集员状态估计方案比较，评估其在现实模拟基准中的保守性。
[0289]
在接下来的时间里，在任何可能的地方删除索引以提高可读性。
[0290]
6.1.初始化
[0291]
由于gnss码相位双差观测值的高线性度，可以仅利用关于初始相对位置的极保守的猜测来初始化滤波器。
[0292]
6.2.传播
[0293]
我们在两个运载工具a和b之间传播保证的相对位置x集
[0294]
x＝p
b-pa[0295]
中等保守性的集值动态运载工具模型几乎不可用，主要是由于缺乏已建立的集合理论系统识别方法。因此，非常期望与运载工具无关的传播模型。集员惯性导航是值得进一步研究的一种选项，但在此不作进一步探讨。相反，我们依赖时差gnss载波相位双差观测值(当可用时)，以及有时没有gnss观测值的粗略速度范数约束模型。接收到的独立的时差载波相位观测值最初被引入用于精确的飞行轨迹重建。在此基础上，最近首次提出了时差双相位差(time-differenced double phase difference，t3d)观测值，用于紧密集值的相关轨迹跟踪。毫米级载波噪声允许计算gnss观测值之间的位置增量的极窄范围。在应用于相对位置估计的情况下，传播的状态集的保守性较小，从而使得在更新步骤中的线性化误差集较小，且最终使得后验状态集的保守性较小。
[0296]
6.2.1.t3d观测值
[0297]
形成载波相位观测值的单差消除了大部分的大气误差和卫星时钟误差。
[0298]
接着，两个接收器a和b以及卫星s的单差载波相位观测值由以下等式给出
[0299]
δφ(pa，pb，ps)＝||p
a-ps||-||p
b-ps||+δnλ+∈
δφ
+δtc
ꢀꢀꢀ
(75)
[0300]
其中，∈
δφ
捕获由硬件噪声和多径引起的残差，δt是差分接收器时钟误差，c是光速，λ是载波波长，且δn是差分载波周期模糊度。
[0301]
在使用ds＝p
a-ps和pb＝pa+x的情况下，我们更紧凑地写为
[0302]
δφ(x，ds)＝||ds||-||ds+x||+δnλ+∈
δφ
+δtc
ꢀꢀꢀ
(76)
[0303]
通过形成相对于参考卫星r的双差来消除差分接收器时钟误差
[0304][0305]
因此，双差载波相位观测值是接收器a和b的相对位置以及绝对接收器位置与卫星
轨道位置之间的向量的非线性函数。
[0306]
假设接收器在两个后续时段内维持相位锁定，模糊度是通过形成两个后续双差观测值的差而消除的常数，从而形成t3d观测值
[0307][0308]
下面我们将展示我们如何计算包围位置增量δx＝x(tk)-x(t
k-1
)的c-全对称多胞形。
[0309]
直接线性化(利用等式78)并利用如e
·
斯科尔特等人在“用于在线应用的非线性集员滤波器(a nonlinear set-membership filter for online applications)”中提出的区间算术计算拉普拉斯余数的区间范围，会由于高相依性而产生非常大的过逼近。
[0310]
这个问题可以通过替代地线性化单差等式76来克服。
[0311]
接着利用ia来评估拉格朗日余数，并将所得区间添加到硬件误差区间∈
δφ
。
[0312]
为了简化必要的雅可比(jacobian)和海塞(hessian)的计算，方便地将等式76写为
[0313][0314]
其中，增强的状态向量为
[0315][0316]
且
[0317]
e1＝[i
303
]，e2＝[i3i3]，s＝[1
ꢀ‑
1]
ꢀꢀꢀ
(83)
[0318]
接着，等式81的雅可比和海塞可以方便地形成为
[0319][0320][0321]
因此，通过评估以下等式，可以获得由于线性化和参数不确定性(卫星的不确定轨道位置)引起的观测值模型误差的区间界限
[0322][0323]
我们继续形成线性单差观测值
[0324]
δφ＝δφ(mid([ds])，mid([x]))
ꢀꢀꢀ
(88)
[0325]
+pdvδφx(x-mid([x]))+r
δφ
+∈
δφ
+δnλ+δtc
[0326]
ꢀꢀꢀ
(89)
[0327]
由于[r
δφ
]通常非常窄，大约为毫米级，用于小间距，我们可以将x＝0线性化并具有如下更紧凑的形式
[0328]
δφ＝hsx+∈
′
δφ
+δnλ+δtc
ꢀꢀꢀ
(90)
[0329]
其常见于rtk文献，其中，∈
′
δφ
＝∈
δφ
+r
δφ
+pdvδφx(x-mid([x]))。接着我们可以形成无相依性的线性双差观测值模型
[0330][0331]
其中，现在将观测值误差和线性化误差归并到一起。
[0332]
取观测值时间分别为t
k-1
和tk的两个连续载波相位观测值的差，则消除了常数模糊度项。在引入δx(tk)＝x(tk)-x(t
k-1
)的情况下，t3d观测值由以下等式给出
[0333][0334]
其中
[0335][0336][0337]
我们在这里利用以下事实：由于接收器与卫星之间的距离较大，几何矩阵在两个连续时间步长之间的变化很小，从而得到非常窄的增量项我们获得了在两个时间步长之间的增量位移中呈线性的观测值。我们利用这种观测值来计算如下的增量位移的极准确范围。在将项归并在一起的情况下，我们将等式96写为如下的线性包含
[0338][0339]
t3d观测值将δx约束为多胞体p，其半空间表示由以下等式给出
[0340][0341]
pδx≤k
ꢀꢀꢀ
(101)
[0342]
接着，相应的c-全对称多胞形z
δx
(tk)由j
·k·
斯科特等人在“受限制全对称多胞形：一种基于集合的估计和故障检测新工具(constrained zonotopes：a new tool for set-based estimation and fault detection)”中公开的定理1给出。通过设置f(t
k-1
)＝i3、b(t
k-1
)＝i3、z
δx
(tk)、zq(t
k-1
)＝0在滤波器传播中使用此对应c-全对称多胞形。
[0343]
6.2.2.速度范数传播
[0344]
在本应用示例中，运载工具间的测距观测值与gnss观测值不同步，并且以更高的速率到达。为了在利用测距观测值更新之前执行必要的传播，利用基于范数有界相对速度的粗略模型。对于传播视距t
k-t
k-1
，范数界限v
max
定义了半径为r＝v
max
(t
k-t
k-1
)的球体。通过具有用户定义的复杂度的对称c-全对称多胞形zs来对这个球体进行外逼近。接着通过设置以下等式执行传播
[0345]
f(t
k-1
)＝i3、b(t
k-1
)＝i3、zu(t
k-1
)＝0、zq(t
k-1
)＝zs。
[0346]
6.3.更新
[0347]
6.3.3.gnss更新
[0348]
gnss码相位单差观测值由以下等式控制
[0349]
δρ(pa，pj，ps)＝||p
a-ps||-||p
b-ps||+∈
δρ
+δtc
ꢀꢀꢀ
(102)
[0350]
其中，∈
δρ
捕获由硬件噪声和残留大气误差引起的残差，δt是差分接收器时钟误差，且c是光速。
[0351]
在利用ds＝p
a-ps和pb＝pa+x的情况下，我们更紧凑地写为
[0352]
δρ(x，ds)＝||ds||-||ds+x||+∈
δρ
+δtc
ꢀꢀꢀ
(103)
[0353]
通过形成相对于参考卫星r的双差来消除差分接收器时钟误差
[0354][0355]
类似于载波相位双差，双差码相位观测值因此是a和b的相对位置以及绝对运载工具位置与卫星轨道位置之间的向量的非线性函数。我们遵循与载波相位观测值相同的策略来形成码相位双差的线性包含，从而得到
[0356][0357]
其允许更新状态范围。
[0358]
6.3.2.测距更新
[0359]
运载工具间的测距观测值由以下等式给出
[0360][0361]
其中，uwb测距天线中心相对于gnss天线中心在相应主体框架中的位置为ra、rb，装置硬件测量值噪声为由于两个uas的姿态ra、rb的不确定性以及不确定的天线位置ra、rb，等式106受到参数不确定性的影响。
[0362]
在下文中，假设ra、rb很小，即测距天线接近gnss天线，并且已经确定其对距离观测值的附加效应的区间界限并将其归并到组合的距离误差wr中：
[0363]
r＝||x||+wrꢀꢀꢀ
(107)
[0364]
为了线性化等式107，我们首先将距离观测值写为
[0365]
r＝(x
t
x)
1/2
+wrꢀꢀꢀ
(108)
[0366]
等式108的一阶区间泰勒展开式与gnss载波相位观测值相同
[0367][0368]
其中，雅可比和海塞为
[0369][0370][0371]
在几何上，这种线性化距离观测值对应于投影在梯度向量上的相对位置向量，而非线性倾斜距离对应于球体。虽然在远距离上是良好的逼近，但随着运载工具彼此接近，球体曲率的增加会引入更大的线性化误差，使得紧密范围变得更加重要。
[0372]
由于x中的多重相依性，利用区间算术应用评估等式109的拉格朗日余数的一般方法提供了较大的过逼近。
[0373]
线性化误差区间越宽，线性化观测值有助于收缩保证的状态集的可能性就越小。
[0374]
在下文中，示出了通过显式评估非线性观测值等式107与其线性化之间的误差，利用单调性，可以获得更紧密的界限。
[0375]
显式线性化误差区间由以下等式给出
[0376][0377]
为了验证等式111相对于[x]的单调性，考虑区间值梯度
[0378][0379][0380]
在几何上，这是两个单位向量之间的差。表达式(111)因此是分段单调的，因为(112)的元素的符号在[x]的子区间上是恒定的。由于这些子区间的数量最多为23，其对应于最多27个唯一顶点。通过针对这些顶点中的每一个评估等式(111)，可以计算出[∈r]的精确范围。为了量化利用分段单调性的益处，我们使用两种方法来计算区间并比较其相应半径。图16显示了在基准任务(参见第6.4部分)过程中的这一测量，以及通过针对从先验状态c-全对称多胞形区间范围中提取的大量样本评估等式(111)获得的区间的内逼近。
[0381]
应注意，一般方案产生的区间半径是利用分段单调性的方案的四倍大，这反过来又非常接近通过采样获得的区间。
[0382]
6.4.模拟
[0383]
我们在基准uas操纵上评估滤波器。其由两个uas组成：一领导者和一跟随者。跟随者在圆形轨迹上从x轴的右侧到左侧改变其相对位置，参见图17。在1hz下跟踪两个智能体的相对位置。模拟的gnss和测距观测值混杂有所记录的观测值误差。对于gnss观测值，这些误差是从两个低成本单频gps接收器的零基线测量值中获得的。测距观测值误差是从放置在已知距离处的两个静态uwb测距信标获得的。
[0384]
6.4.1.集合体积
[0385]
在所有三个场景中，czesmf滤波器提供比迭代扩展集员滤波器(iterated extended set-membership filter，iesmf)和sivia算法的向量实施(vector implementation of sivia，vsivia)更小的集合体积。与粗略的速度范数界限模型相比，我
们还评估了利用t3d观测值进行传播的益处。如图18所示，通过在传播步骤中使用载波相位信息，c-全对称多胞形体积可以减少几乎一个数量级。
[0386]
6.5.对卫星群集的灵敏度
[0387]
相对位置范围的可以实现大小和形状取决于基线卫星的几何形状，如由熟悉的精度衰减(dop)准确度测量所捕获。因此，了解所提出的集员滤波器的相对保守性对于卫星群集的变化是否稳健是有意义的。对一组随机选择的gps轨道记录执行基准操纵模拟。卫星位置是根据每个轨道记录前两分钟的样条拟合计算出来的。图19示出由vsivia获得的子铺设体积进行归一化后的czesmf和iesmf的所得相对位置集合体积。在初始转换之后，czesmf在所有模拟中展示出比vsivia更小的保守性，并且相对于gnss轨道的变化非常小。iesmf明显更加保守，对卫星轨道的灵敏度也同样低。这些模拟提供了一些信心，即所观测到的由czesmf计算出的范围的优良紧密度有望延续到滤波器的在线实施中。
[0388]
7.结论
[0389]
上述用于基于c-全对称多胞形的非线性系统的集合状态估计方法在模拟场景中明显优于(在集合体积方面)基于vsivia的非线性离线集合估算器以及最近的快速椭球集合状态估算器。
[0390]
所提出的滤波器可应用于各种估计问题，包含同步定位和映射(slam)，鉴于当前对道路运载工具安全视觉定位的关注，这是一个令人兴奋的前景。
[0391]
附录a：用c-全对称多胞形逼近2维或3维球
[0392]
为了逼近以c为中心的半径为r的球，我们用h-rep计算逼近并将其转化为c-全对称多胞形。
[0393]
给定在区间中均匀分布的角度为θ＝(θ1,θ2,
…
,θ
l
)的向量和在[0,2π]中均匀分布的角度为φ＝(φ1,φ2,
…
,φ
l
)的向量，其中θ为天顶角，φ为方位角，且l为复杂度参数，针对每个角度组合(θi,φj)计算对应的法向量
[0394][0395]
这些法向量中的每一个定义了半空间。接着可以很容易地用h-rep写出相应的多胞体
[0396][0397]
并利用j
·k·
斯科特等人在“受限制全对称多胞形：一种基于集合的估计和故障检测新工具(constrained zonotopes:a new tool for set-based estimation and fault detection)”中给出的方法将其转化为g-rep.。
[0398]
在二维中，对在[0,2π]上均匀分布的单组角度θ＝(θ1,θ2,
…
,θ
l
)执行相同的方法。