Geometric Symmetry Reduction of the Unobservable Subspace for Kalman Filtering

TL;DR

This paper introduces a geometric approach to reduce the unobservable subspace in Kalman filtering for invariant dynamical systems, improving accuracy and robustness in orientation and position estimation.

Contribution

It presents a novel reduction technique for unobservable subspaces in Kalman filters using geometric symmetry, enhancing estimation accuracy and robustness.

Findings

Improved orientation and position estimation accuracy.

Enhanced robustness to measurement noise.

Effective decomposition of state space into observable and unobservable parts.

Abstract

In this article, we consider the implications of unobservable subspaces in the construction of a Kalman filter. In particular, we consider dynamical systems which are invariant with respect to a group action, and which are therefore unobservable in the group direction. We obtain reduced propagation and measurement equations that are invariant with respect to the group action, and we decompose the state space into unobservable and observable parts. Based on the decomposition, we propose a reduced Bayesian inference method, which exhibits superior accuracy for orientation and position estimation, and that is more robust to large measurement noise.

Figures

Tables

Table 1. Table 1 . Average error, straight line trajectory, average feature num = 20

	Straight line trajectory, average feature num = 20
Frequency	update frequency = 10Hz						update frequency = 20Hz
Method	EKF		FE-EKF		Reduced EKF		EKF		FE-EKF		Reduced EKF
Error	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$
$Q_z=1\times {10}^{-4}$	1.403	0.0375	1.539	0.0421	1.436	0.0373	1.365	0.0308	1.456	0.0345	1.300	0.0282
$Q_z=2\times {10}^{-4}$	2.492	0.0487	2.525	0.0552	2.549	0.0506	2.395	0.0411	2.621	0.057	2.397	0.0432
$Q_z=4\times {10}^{-4}$	4.332	0.0752	4.284	0.0758	4.449	0.0773	4.265	0.0578	4.283	0.0677	4.317	0.0644

Table 2. Table 2 . Average error, straight line trajectory, average feature num = 40

	Straight line trajectory, average feature num = 40
Frequency	update frequency = 10Hz						update frequency = 20Hz
Method	EKF		FE-EKF		Reduced EKF		EKF		FE-EKF		Reduced EKF
Error	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$
$Q_z=1\times {10}^{-4}$	1.303	0.0269	1.306	0.027	1.256	0.0227	1.21	0.0167	1.283	0.0238	1.258	0.0208
$Q_z=2\times {10}^{-4}$	2.409	0.0385	2.459	0.0446	2.43	0.0347	2.331	0.0279	2.318	0.0313	2.386	0.0345
$Q_z=4\times {10}^{-4}$	4.238	0.047	4.361	0.0628	4.386	0.0613	4.23	0.0445	4.353	0.0558	4.352	0.0577

Table 3. Table 3 . Average error, circle trajectory, average feature num = 20

	Average error, circle trajectory, average feature num = 20
Frequency	update frequency = 10Hz						update frequency = 20Hz
Method	EKF		FE-EKF		Reduced EKF		EKF		FE-EKF		Reduced EKF
Error	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$
$Q_z=1\times {10}^{-4}$	0.955	0.0875	1.063	0.0936	0.655	0.0443	0.746	0.0657	0.750	0.0576	0.532	0.0342
$Q_z=2\times {10}^{-4}$	1.630	0.152	1.496	0.119	0.959	0.0586	1.35	0.128	1.549	0.144	0.982	0.0667
$Q_z=4\times {10}^{-4}$	2.893	0.2914	2.721	0.2585	1.512	0.1033	2.295	0.2256	2.066	0.2102	1.469	0.1003

Table 4. Table 4 . Average error, circle trajectory, average feature num = 40

	Average error, circle trajectory, average feature num = 40
Frequency	update frequency = 10Hz						update frequency = 20Hz
Method	EKF		FE-EKF		Reduced EKF		EKF		FE-EKF		Reduced EKF
Error	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$
$Q_z=1\times {10}^{-4}$	0.8161	0.0643	0.7972	0.0627	0.5815	0.0359	0.5843	0.0462	0.681	0.0530	0.568	0.0374
$Q_z=2\times {10}^{-4}$	1.2762	0.1193	1.3061	0.1091	0.9403	0.0591	1.2603	0.1164	1.2402	0.1039	0.9178	0.0573
$Q_z=4\times {10}^{-4}$	2.593	0.253	2.428	0.236	1.752	0.121	2.4	0.239	2.397	0.216	1.631	0.113

Table 5. Table 5 . Average error, general trajectory, average feature num = 25

	Average error, general trajectory, average feature num = 25
Frequency	update frequency = 10Hz						update frequency = 20Hz
Method	EKF		FE-EKF		Reduced EKF		EKF		FE-EKF		Reduced EKF
Error	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$
$Q_z=1\times {10}^{-4}$	0.673	0.0789	0.635	0.0803	1.309	0.1062	0.6527	0.0698	0.5620	0.0605	0.4659	0.0371
$Q_z=2\times {10}^{-4}$	1.388	0.139	1.143	0.12	1.23	0.107	1.2575	0.1156	0.9985	0.0997	0.8022	0.0654
$Q_z=4\times {10}^{-4}$	2.459	0.227	2.111	0.2267	1.592	0.1174	2.3852	0.2039	1.8924	0.1794	1.4595	0.1042

Table 6. Table 6 . Average error, general trajectory, average feature num = 50

	Average error, general trajectory, average feature num = 50
Frequency	update frequency = 10Hz						update frequency = 20Hz
Method	EKF		FE-EKF		Reduced EKF		EKF		FE-EKF		Reduced EKF
Error	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$	$\delta p$	$\delta \theta$
$Q_z=1\times {10}^{-4}$	0.6039	0.0609	0.5815	0.0636	1.308	0.1049	0.5518	0.0518	0.4810	0.0447	0.4538	0.0381
$Q_z=2\times {10}^{-4}$	1.2201	0.1047	0.9836	0.0945	1.0738	0.0838	1.1676	0.1021	0.9752	0.0869	0.73	0.0611
$Q_z=4\times {10}^{-4}$	2.4264	0.2126	2.0421	0.1904	1.4674	0.1121	2.447	0.2141	1.8347	0.145	1.4322	0.1165

Equations

$$\Phi (x_{k+s},x_{k+s-1})\dots \Phi (x_{k+2},x_{k+1})\cdot \Phi (x_{k+1},x_k){\mathcal{N}}_k=\Phi (x_{k+s},x_k){\mathcal{N}}_k\perp dh(x_{k+s}),$$

$$p(x_N,x_O)=p(x_O)\cdot p(x_N\mid x_O),$$

$$\overline{p}(x_N,x_O)=\overline{p}(x_O)\cdot p(x_N\mid x_O),$$

$$\{e_1,e_2\}=R(\theta )=\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right).$$

$$X=(p,\theta ,p_1^f,p_2^f,\dots p_N^f).$$

$$\{\begin{array}{rl}\dot{p}& =R(\theta )v,\\ \dot{\theta }& =w,\\ {\dot{p}}_i^f& =0;\end{array}$$

$$z_i=R(\theta {)}^T(p_i^f-p),$$

$$\{\begin{array}{rl}{x}^{\prime }& =f(x)+\sum _{i=1}^n{g}_i(x)u_i,\\ z& =h(x),\end{array}$$

$$\Omega =⟨f,g_1,g_2\dots ,g_n\mid dh⟩.$$

$$\{\begin{array}{rl}{x}_1^{\prime }& ={\overline{f}}_1(x_1,x_2)+\sum _{i=1}^n{\overline{g}}_{1i}(x_1,x_2)u_i,\\ x_2^{\prime }& ={\overline{f}}_2(x_2)+\sum _{i=1}^n{\overline{g}}_{2i}(x_2)u_i,\\ z& =\overline{h}(x_2).\end{array}$$

$${\Omega }^{\perp }=\mathcal{N}=\text{span}\left(\begin{array}{cc}{I}_2& Jp\\ \text{0}& 1\\ I_2& J{p}_1^f\\ I_2& J{p}_2^f\\ ⋮& ⋮\\ I_2& J{p}_N^f\end{array}\right),$$

$$J=\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right).$$

$$\{\begin{array}{rl}p& \mapsto p+\Delta p,\\ \theta & \mapsto \theta ,\\ p_i^f& \mapsto p_i^f+\Delta p;\end{array}\{\begin{array}{rl}p& \mapsto R(\Delta \theta )p,\\ \theta & \mapsto \theta +\Delta \theta ,\\ p_i^f& \mapsto R(\Delta \theta )p_i^f,\end{array}$$

$$\{\begin{array}{rl}{p}_{k+1}& =p_k+\Delta t\cdot R({\theta }_k)(v_m^{k+1}+n_v),\\ {\theta }_{k+1}& ={\theta }_k+\Delta t\cdot (w_m^{k+1}+n_w),\\ p_{i,k+1}^f& =p_{i,k}^f,\end{array}$$

$$\{\begin{array}{rl}{p}_{k+1}& =p_k+R({\theta }_k)\Delta d_{k+1},\\ {\theta }_{k+1}& ={\theta }_k+\Delta {\theta }_{k+1},\\ p_{i,k+1}^f& =p_{i,k}^f,\end{array}$$

$$\Phi =\left(\begin{array}{ccc}{I}_2& JR({\theta }_k)\Delta d_{k+1}& \\ \text{0}& 1& \\ & & I_{2N}\end{array}\right)=\left(\begin{array}{ccc}{I}_2& J(p_{k+1}-p_k)& \\ \text{0}& 1& \\ & & I_{2N}\end{array}\right)=\Phi (x_{k+1},x_k),$$

$$d{h}_i(x)$$

$$=R(\theta {)}^T\left(\begin{array}{ccccc}-I_2,& J^T(p_i^f-p),& \text{0},& \dots & I_2\dots \end{array}\right),$$

$$\Phi (x_{k+1},x_k)\cdot {\mathcal{N}}_k=\left(\begin{array}{ccc}{I}_2& J(p_{k+1}-p_k)& \\ \text{0}& 1& \\ & & I_{2N}\end{array}\right)\cdot \left(\begin{array}{cc}{I}_2& J{p}_k\\ \text{0}& 1\\ I_2& J{p}_{1,k}^f\\ I_2& J{p}_{2,k}^f\\ ⋮& ⋮\\ I_2& J{p}_{N,k}^f\end{array}\right)=\left(\begin{array}{cc}{I}_2& J{p}_{k+1}\\ \text{0}& 1\\ I_2& J{p}_{1,k}^f\\ I_2& J{p}_{2,k}^f\\ ⋮& ⋮\\ I_2& J{p}_{N,k}^f\end{array}\right),$$

$$\{\begin{array}{rl}{x}^{\prime }& =f(x,u),\\ z& =h(x),\end{array}$$

$$\{\begin{array}{rl}(g\cdot x{)}^{\prime }& =f(g\cdot x,u),\\ z& =h(g\cdot x).\end{array}$$

$$\text{Orb}(x)=\{g\cdot x\mid g\in G\},$$

$$\pi :X\to X/G$$

$$\{\begin{array}{rl}{}^{\prime }& =f([x],u),\\ z& =h([x]).\end{array}$$

$$z_i=\text{Id}(z_i);$$

$$\frac{d}{dt}{z}_i$$

$$=R(\theta {)}^T({\dot{p}}_i^f-\dot{p})+J^{T}R(\theta {)}^T\dot{\theta }(p_i^f-p)$$

$$=R(\theta {)}^T(-R(\theta )v)+J^{T}R(\theta {)}^{T}w(p_i^f-p)$$

$$=-v-wJ{z}_i.$$

$$\{\begin{array}{rl}\dot{p}& =R(\theta )v,\\ \dot{\theta }& =w,\\ \dot{z_i}& =-v-wJ{z}_i,\end{array}$$

$$z_i=\text{Id}(z_i).$$

$${\Omega }^{\perp }=\mathcal{N}=\text{span}\left(\begin{array}{cc}{I}_2& Jp\\ \text{0}& 1\\ \text{0}& 0\\ \text{0}& 0\\ ⋮& ⋮\\ \text{0}& 0\end{array}\right).$$

Full text

1. Introduction

Kalman filters are widely used in practical state estimation problems, including SLAM (Simultaneous Localization and Mapping). This is primarily due to its conceptual simplicity and the low computational complexity compared to optimization based methods. Various types of sensors are used for localization tasks, including GPS, odometry, inertial measurement units (IMU), cameras, Light Detection and Ranging (LIDAR), and so on, and the specific choice of sensors used depends on the application. Kalman filters have been successfully applied to Visual Odometry (VO) [2], Visual Inertial Odometry (VIO) [12], and more recently, to autonomous driving cars, where information from multiple sensors (GPS, odometry, IMU, and cameras) and HD maps are integrated together to yield a more robust and accurate result.

A Kalman filter is derived based on the assumptions that the probability distribution is Gaussian and the dynamics are linear. However, practical models generally involve nonlinear dynamics and non-Gaussian distributions, and for such applications, Kalman filters are less accurate than optimization based methods.

There have been ongoing efforts to improve the performance of Kalman filters. The unscented Kalman filter (UKF) [14] is used to reduce the local linearization error of the extended Kalman filer (EKF); inverse depth parametrization [11] is used to represent the three-dimensional spatial location of feature points recognized by camera, as a Gaussian distribution on Euclidean space about a feature point’s position does not model its depth uncertainty well; the multi-state constraint Kalman filter (MSCKF) [12] method only maintains a sliding window of historic camera poses, and removes feature points from the state space. Multiple views of the same feature point gives a constraint on the state variable, and serves as measurement. Such an approach can be viewed as an attempt to further relax the assumption that the uncertainty in the feature point positions are described by Gaussian distributions and thereby improve the algorithm’s performance.

There is also an effort to improve the consistency of the Kalman filter by taking into account the issue of observability. The EKF does not respect the observability properties of the underlying continuous control system, which is believed to be a major source of inconsistency in the state estimate. It is hoped that by correctly incorporating the observability property into the construction of the algorithm, the consistency and hence accuracy of the method will be improved. An example of this phenomena is given in [7], where a stationary robot measured the relative position of new landmarks multiple times, and the covariance of robot’s pose estimate becomes smaller over time. This is contrary to our intuition, as the the robot is static and therefore new landmarks do not provide new information about the robot’s pose, and re-observation of these landmarks should not affect robot’s own pose estimation. So, the pose of the robot is unobservable given such measurements, but the algorithm mistakenly treats it as an observable variable.

This phenomena has been studied in depth using observability analysis [4]. It turns out three dimensions of the state space, i.e., the robot’s position and its orientation, are unobservable to odometry and camera measurements, while for the linearized discrete model, only two dimensions (the robot’s position) remain unobservable. This was addressed by considering two modifications, one is the special transition matrix $\Phi(\check{x}_{k+1},\hat{x}_{k})$ that depends on $\hat{x}_{k}$ (modified state value at $k$-th step) and $\check{x}_{k+1}$ (predicted state value at $(k+1)$-th step). If we denote the unobservable subspace at $\hat{x}_{k}$ by $\mathcal{N}_{k}$, then after the measurement update, $\Phi(\check{x}_{k+1},\hat{x}_{k})\mathcal{N}_{k}$ is still perpendicular to $dh(\check{x}_{k+1})$. More importantly, this transition matrix satisfies $\Phi(x_{k+2},x_{k+1})\cdot\Phi(x_{k+1},x_{k})=\Phi(x_{k+2},x_{k})$, thus along the true trajectory, we have

[TABLE]

which means the unobservable subspace remains unobservable during the propagation, which is a discrete version of the invariance property satisfied by the continuous control system. However, since the Kalman filter is implemented by alternating propagation and measurement update, the Jacobians are calculated at both $\hat{x}_{k}$ and $\check{x}_{k}$, and as a consequence, property (1) no longer holds; based on this observation, the other modification is the First Estimate Jacobian (FEJ) framework, where Jacobians are evaluated on the first ever available estimate of all the state variables so to make property (1) hold again. Later, a revised technique termed observability constrained (OC)-EKF [5] was proposed to not only guarantee the desired observability property but also to minimize the expected errors of the linearization points. These techniques were combined with the MSCKF method to obtain MSCKF2.0 [9], which is an efficient algorithm that claims to have performance no worse than optimization-based methods.

In this paper, we try to approach the observability problem from a different perspective. Notice that the Kalman filter is composed of alternating propagation and measurement update steps. While the propagation step is simply a discretization of underlying stochastic differential equation, the measurement update step is essentially Bayesian inference. Such a measurement update step does not exist in the continuous control system when we perform observability analysis, so the propagation of system is “interrupted” at each step, so forcing this discrete system to mimic the observability property of the continuous system is somewhat artificial. Since the measurement update step is essentially Bayesian inference, we would try to solve the observability problem at this step from the perspective of probability theory. The main idea is that by viewing the unobservability of the system as an invariance with respect to a group action, the state space can be decomposed into unobservable and observable parts explicitly, $x=(x_{N},x_{O})$. As in the case of symmetry reduction in geometric mechanics, we can obtain reduced equations that only involve the reduced (or observable) variables, and the propagation and measurement equations will be expressed only in terms of the observable variables $x_{O}$. We claim that in order to keep $x_{N}$ unobservable during the algorithm, i.e., avoid introducing spurious information from measurements to $x_{N}$, Bayesian inference should only be employed on the $x_{O}$ part, which yields a reduced measurement model. Mathematically, it means, for a probability distribution $p(x_{N},x_{O})$, which can be factored into

[TABLE]

the conditional distribution $p(x_{N}\mid x_{O})$ shall remain unchanged during the measurement update, and only $p(x_{O})$ can be improved by the reduced measurement. So after the measurement update

[TABLE]

where $\overline{p}(x_{O})$ is the improved distribution for the observable variables. Even though measurement is not applied directly to $x_{N}$, improvement in the estimation of $x_{O}$ helps to improve the estimation of $x_{N}$ implicitly via the conditional probability $p(x_{N}\mid x_{O})$, which is a consequence of the correlation between $x_{N}$ and $x_{O}$. A geometric picture may help to understand this, the unobservable subspaces are tangent spaces to a collection of submanifolds in state space, and $x_{N}$ denotes the degrees of freedom on these submanifold. Each point on a given submanifold will yield the same measurement results. The $x_{O}$ indicates which submanifold the state is in, and the different submanifolds are distinguishable using the measurement data. Suppose we have a probability distribution on state space, if we are more certain about $x_{O}$, i.e., which submanifold we are in, it also improves our estimate about $x_{N}$ as long as they are not independent. By using this framework, the inconsistency counterexample in [7] is naturally avoided. In that case, the unobservable variables are $x_{N}=(p,\theta)$, the position and orientation of robot, and the observable variables are $x_{O}=(z_{1},z_{2}\dots,z_{N})$, where $z_{i}$ is the relative position of $i$-th feature point. When new features are observed, new $z_{i}$ will be appended to the state variables, and they are independent from the old state variables. As long as the robot remains static, i.e., no propagation of the uncertainty is performed, no matter how often we re-observe these feature points and improve their relative position estimation, it will not affect the pose of the robot itself.

In Section 2, we review the planar robot model from [5], together with its observability property. We stress the use of group actions to characterize unobservable subspaces. In Section 3, we discuss how to obtain a reduced control system by considering the invariant group action, and how to decompose the planar robot system into the $x_{N}$ and $x_{O}$ parts. In Section 4, we choose an appropriate Riemannian structure for the new coordinate system, and implement reduced Bayesian inference (2) for the Gaussian distribution. Numerical experiments are given in Section 5, where straight line, circle and general trajectories are tested, and the reduced EKF method demonstrates superior performance when compared to the classical EKF and FE-EKF method.

2. Background

A planar robot is considered, equipped with odometry and cameras, navigating in an unknown environment. The odometry measures local velocity and angular velocity of the robot, while cameras detect feature points and measure their relative position to the robot. To make the notation simple and the numerical simulation easier to implement, we assume that the robot is equipped with binocular cameras, thus at each instant, measurements from two cameras will be combined to give an estimate of the relative position of feature points, and this serves as our measurement. Fix a global frame, and assign an intrinsic frame $\{e_{1},e_{2}\}$ to the robot. The robot pose is denoted by $(p,\theta)$, where $p$ is the global position of the robot, and $\theta$ represents the relative angle from the global frame to the intrinsic frame. Thus, the intrinsic frame of the robot is given by

[TABLE]

The global position of feature points are denoted by $(p^{f}_{1},p^{f}_{2},\dots p^{f}_{N})$, and together, they form the classical state space of this problem

[TABLE]

The total dimension of $X$ is $3+2N$, and this state space has the structure $SE(2)\times\mathbb{R}^{2N}$, where $SE(2)$ is the two-dimensional Euclidean group. Denote the local velocity and angular velocity measured using odometry by $v$ and $w$, and the relative position of the $i$-th feature point by $z_{i}$. Then, the underlying continuous control system is given by

[TABLE]

[TABLE]

where (3) is the propagation equation, and (4) is the measurement equation.

As we have already mentioned, there are three unobservable dimensions in this model, the robot position $p$ and its orientation $\theta$. There are two main ways to detect unobservable dimensions in a given control system. One is the observability rank criterion [3]. Given a general affine control system

[TABLE]

regard $f(x)$, $g_{1}(x)$, $g_{2}(x)$, $\dots$, $g_{n}(x)$ as vector fields on the state space $X$, then we construct the minimal dual distribution that is invariant under $f(x)$, $g_{1}(x)$, $g_{2}(x)$, $\dots$, $g_{n}(x)$, which is given by

[TABLE]

This step involves repeated Lie derivative calculations, and is analogous to computing the observability matrix for constant linear systems. Its orthogonal distribution $\Omega^{\perp}$, which is analogous to the null space of the observability matrix, defines the unobservable subspace for this system. There exists a corresponding local coordinate transformation $\phi(x)=(x_{1},x_{2})$ to the distribution $\Omega^{\perp}$, such that the system (5) can be decomposed as follows

[TABLE]

It is clear that the measurement only depends on the $x_{2}$ part, and $x_{2}$ evolves by itself, which is unaffected by $x_{1}$. For any solution $(x(t),u(t),z(t))$ that satisfies the system (5), if we move the initial state $x(t_{0})$ along the unobservable submanifold that passes through it, i.e., by only changing the $x_{1}$ part, the new trajectory corresponds to the same control $u(t)$ and measurement $z(t)$, and thus $x_{1}$ is unobservable in this system. Any physical property of this system that is observable has to be independent of $x_{1}$, in other words, has to be constant along each unobservable submanifold. It has been verified in [5] that for the system (3), (4),

[TABLE]

where

[TABLE]

The first two columns indicate that the robot position $p$ is unobservable, and the third column indicates that the robot orientation is also unobservable.

The disadvantage of the observability rank criterion technique is it involves massive amounts of Lie derivative calculations. An easier way to detect unobservable dimensions is by invariant group actions. In classical mechanics, for Hamiltonian systems, we have the famous Noether’s theorem, which essentially relates symmetries to conserved quantities. For example, if the system is invariant under translation, then the total linear momentum is preserved by the system; if system is invariant under rotations, then the total angular momentum is also be preserved. A similar idea can be applied to observability analysis. If our control system is invariant under some group action, i.e., corresponds to the same control $u(t)$ and measurement $z(t)$, then we have discovered an unobservable dimension of this system. For the planar robot model, we can verify that under the following translation and rotation,

[TABLE]

(3) (4) remains invariant. For each state $(p,\theta,p^{f}_{1},p^{f}_{2},\dots p^{f}_{N})$, we can calculate the tangent subspace induced by the above group action. In particular, the translation action gives the first two columns of (7), and the rotation action gives the third column of (7). It is sometimes natural to find these invariant group actions by physical intuition. Odometry measures local velocity and local angular velocity, while the camera measures relative position of feature points, as such, the information that they provide is “local” and cannot give constraints on the global pose. Global position can be measured by using GPS or if a map provides the absolute position of feature points. In the case of VIO, the IMU provides measurements of the local acceleration and angular velocity. Since the IMU measurement can detect the gravity direction, gravity breaks the full rotational symmetry, so only rotations around the gravity direction keep the system invariant, which indicates that the yaw angle is unobservable in VIO applications. The main disadvantage of the invariant group action technique as a method of characterizing unobservable dimensions is that we cannot determine if we have found all the unobservable dimensions.

We now describe the special transition matrix for (3) that satisfies the chain rule property (1). Let $v=v_{m}+n_{v}$, $w=w_{m}+n_{w}$, where $v_{m}$,$w_{m}$ are odometry measurements, and $n_{v}$, $n_{w}$ are measurement noise. A simple forward Euler integration of (3) gives

[TABLE]

so, the nominal values are updated as follows

[TABLE]

with $\Delta d_{k+1}=v_{m}^{k+1}\cdot\Delta t$, $\Delta\theta_{k+1}=w_{m}^{k+1}\cdot\Delta t$. The transition matrix for (10) is

[TABLE]

The derivative of the measurement (4) is given by

[TABLE]

where $I_{2}$ appears at the $i$-th index of the feature points. We first check that $dh_{i}\cdot\mathcal{N}=0$, which is theoretically guaranteed; then the nullspace $\mathcal{N}_{k}$ at $x_{k}$ after applying the transition matrix $\Phi(x_{k+1},x_{k})$ is

[TABLE]

it is easy to verify that $dh_{i}(x_{k+1})\cdot\Phi(x_{k+1},x_{k})\cdot\mathcal{N}_{k}=0$, since $p^{f}_{i,k+1}=p^{f}_{i,k}$. Finally, we can also verify that transition matrix $\Phi(x_{k+1},x_{k})$ (11) satisfies $\Phi(x_{k+2},x_{k})=\Phi(x_{k+2},x_{k+1})\cdot\Phi(x_{k+1},x_{k})$, so the chain rule property (1) is satisfied along the exact trajectory.

3. Geometric reduction

Given the state space $X$, and a general control system

[TABLE]

suppose we have a left Lie group action of $G$ on $X$, i.e., $G\times X\to X$, which keeps the system (12) invariant, i.e., for any $(x(t),u(t),z(t))$ that satisfies (12), $(g\cdot x(t),u(t),z(t))$ also satisfies (12) for $\forall g\in G$, i.e.,

[TABLE]

For each point $x\in X$, $\forall\xi\in\mathfrak{g}$, where $\mathfrak{g}$ is the Lie algebra of $G$, the infinitesimal generator $\xi_{X}(x)=\left.\frac{d}{dt}\right|_{t=0}\text{exp}(\xi(t))\cdot x$ gives one unobservable direction of $x$, and the orbit of $x$,

[TABLE]

gives the unobservable submanifold that passes through $x$. This invariant group action on the control system allows us to perform reduction to express the system in terms of reduced variables on the quotient space $X/G$. This kind of reduction due to the presence of a continuous symmetry group arises in many disciplines. In optimization, when objective function is invariant under a group action, the problem can be rephrased as an optimization problem on the quotient space [1]; also for classical mechanics on a Lie group, when Lagrangian or Hamiltonian is invariant under the group action, the mechanics can be reduced to the Lie algebra $\mathfrak{g}$ or its dual $\mathfrak{g}^{*}$, which is referred to as Euler–Poincaré reduction and Lie–Poisson reduction [10], respectively. Here, suppose that the group action $G\times X\to X$ is free and proper, then we obtain a smooth quotient space $X/G$, with a quotient map[8]

[TABLE]

that is a smooth submersion. Then, the control system (12) can be reduced to the quotient space $X/G$. For $[x]\in X/G$,

[TABLE]

(14) is a reduced control system with reduced propagation and measurement equations. Consider (3), (4) as a concrete example. The state space is $X=SE(2)\times\mathbb{R}^{2N}$, and we already know that the group action $G=SE(2)$ (8) on $X$ leaves (3) (4) invariant, so this induces a reduced control system on $X/G$. However, $X/G$ is an abstract quotient manifold, so in order to deal with it explicitly, we need a concrete coordinate representation. It turns out that the relative position of feature points $z_{i}$ provide a natural coordinate representation for $X/G$. With these coordinates for the reduced space, the measurement equation (4) reduces to

[TABLE]

and the propagation equation (3) reduces to

[TABLE]

Together with the robot position $p$ and its orientation $\theta$, we obtain a new coordinate representation of the state space, $(p,\theta,z_{1},z_{2}\dots,z_{N})$, and the control system with respect to this new coordinate system is given by

[TABLE]

[TABLE]

The coordinate system given by $(p,\theta,z_{1},z_{2}\dots,z_{N})$ is global, and can be regarded as the state space being decomposed into the product of $x_{N}=(p,\theta)$ and $x_{O}=(z_{1},z_{2}\dots,z_{N})$, the former is unobservable part, and the latter is observable part, while the group action (8) now acts trivially on the $x_{O}$ part. In this case, the unobservable subspace at each point can be represented as

[TABLE]

We improved the linearity of the measurement equation (16) by transforming to the relative feature position representation, and as a consequence, the originally trivial propagation equation $\dot{p}^{f}_{i}=0$ now becomes the nontrivial $\dot{z}_{i}=-v-wJz_{i}$.

We can construct a similar transition matrix for (15) as in (11), and apply forward Euler integration to (15), which yields

[TABLE]

and the corresponding nominal value update step is given by

[TABLE]

and the transition matrix is

[TABLE]

The derivative of the reduced measurement (16) is

[TABLE]

and we can see that $dh_{i}\cdot\mathcal{N}=0$, which holds simply because the lower $2N\times 3$ part of $\mathcal{N}$ (17) is zero. This property still holds after repeated measurement updates

[TABLE]

as the bottom left parts of the transition matrices $\Phi_{k+j,k+j-1}$ vanish.

4. Bayesian inference

In Section 3, we discussed how to obtain the reduced control system that arises as a consequence of the invariant group action, which lead to the reduced propagation and measurement equations on the reduced quotient space. This motivated the use of a new coordinate representation $(p,\theta.z_{1},z_{2}\dots z_{N})$ for the planar robot system. This decomposes the state space into the product of the unobservable part $(p,\theta)$, and the observable part $(z_{1},z_{2}\dots z_{N})$. We also constructed a transition matrix which ensures that the unobservable subspace remains perpendicular to the measurement $dh$ during propagation. But, as we noted, this propagation is “interrupted” at each step during the Kalman update. In order to preserve the unobservability property of the system during the measurement update, we propose a reduced Bayesian inference update(2) on the observable part using the reduced measurement. It is natural to apply Bayesian inference on $p(z_{1},z_{2}\dots z_{N})$, and update $p(p,\theta,z_{1},z_{2}\dots z_{N})$ using (2). However, before we do that, there are some basic things that we need to make clear. The first question is, what is Bayesian inference? As we all know, Bayesian inference is

[TABLE]

which arises naturally from the fact that

[TABLE]

where $p(x)$, $p(y)$, $p(x,y)$ are probability density functions. The next question is, what is a probability density function? As we all know, a probability distribution is a probability measure on the state space, and the probability density function is a concrete way to represent it,

[TABLE]

However, in (22) we need to specify a measure $dx$ to perform the integration, and the representation of the probability measure as a probability density function depends on that choice of $dx$. Also, given a probability distribution on state space, when we try to find the most probable point, we find this problem also requires the introduction of a measure on the state space. As such, we need to specify a measure on the state space in order to define the probability density function or to find the most probable point. For a more detailed discussion of such issues, see [6]. After specifying the measure $dx$, the density function associated with a probability measure $\mu$ is just the Radon–Nikodym derivative of $\mu$ with respect to $dx$. A natural way to specify a measure on a smooth manifold is by specifying a Riemannian structure on it. Each Riemannian structure induces a Riemannian volume form, which in turn induces a measure. When we write down the common Gaussian density function, we are actually assuming the standard Riemannian structure on $\mathbb{R}^{n}$ implicitly. This structure is quite natural, as it is homogeneous and isotropic and does not introduce prior information on the space. More precisely, this structure is invariant under Euclidean transformations.

With the observation that each density function is defined with respect to an underlying base measure, we look at (21) again, and find that there is a product measure implicitly defined on $(x,y)$, and that (21) is essentially an application of Fubini’s theorem.

Now consider the new coordinate system $(p,\theta,z_{1},z_{2}\dots z_{N})$ we get for the planar robot system, which is diffeomorphic to the original coordinate system by the following transformations

[TABLE]

Given the decomposition $p(x_{N},x_{O})=p(x_{O})\cdot p(x_{N}\mid x_{O})$, where $x_{N}=(p,\theta)$, $x_{O}=(z_{1},z_{2},\dots z_{N})$, which Riemannian structure should we assign to the state space to obtain a base measure? For the old coordinate system $(p,\theta,p^{f}_{1},p^{f}_{2}\dots p^{f}_{N})$, it is natural to use the standard Riemannian structure $dp\otimes dp+d\theta\otimes d\theta+\sum_{i=1}^{N}dp^{f}_{i}\otimes dp^{f}_{i}$, as it is invariant under the group action (8). Moreover, if a Lie group $G$ acts freely and properly on a Riemannian manifold $M$, and the action is isometric for $\forall g\in G$, then there is a natural Riemannian structure on $M/G$ induced from $M$. For our quotient space $(z_{1},z_{2}\dots z_{N})$, it can be verified that the induced Riemannian structure is simply the standard Riemannian structure of $\mathbb{R}^{2N}$. However, the Riemannian structure of $(p,\theta,z_{1},z_{2}\dots z_{N})$ induced by the diffeomorphism (23) is not simply the product of the standard Riemannian structure of $(p,\theta)$ with the standard Riemannian structure of $(z_{1},z_{2}\dots z_{N})$. This can be verified by checking the Jacobian of (23), which is not a unitary matrix. We now have to decide which Riemannian structure to use on $(p,\theta,z_{1},z_{2}\dots z_{N})$, either choose the non-product structure induced from (23), or choose the product structure. We choose the product structure here, one reason is that it is simpler to apply the reduced Bayesian inference step (2), another reason is this product structure is also invariant under the group action (8), which ensures that $(z_{1},z_{2}\dots z_{N})$ remains fixed.

Finally, we are in a position to discuss how to implement (2) for the Gaussian distribution with respect to the standard Riemannian structure. Consider $p(x_{N},x_{O})$ with a Gaussian distribution given by

[TABLE]

Then, by the property of conditional Gaussian distributions,

[TABLE]

The next step is to obtain an improved $\overline{p}(x_{O})=\mathcal{N}(\overline{\mu}_{O},\overline{\Sigma}_{OO})$ by taking a reduced measurement and computing the improved joint distribution $\overline{p}(x_{N},x_{O})$. In order to compute the joint distribution from the product of two Gaussian distributions efficiently, we choose the precision matrix, information vector representation of the Gaussian distribution,

[TABLE]

where $\Omega_{OO}=\Sigma^{-1}_{OO}$, $\xi_{O}=\Sigma^{-1}_{OO}\cdot\mu_{O}$, and $\Omega_{N\mid O}=\Sigma^{-1}_{N\mid O}$. Suppose that the reduced discrete measurement equation is given by

[TABLE]

with measurement noise $\delta\sim\mathcal{N}(0,Q)$, where $Q$ is the noise covariance, then by the information filter [13],

[TABLE]

Let $\mu_{N}+\Sigma_{NO}\Sigma^{-1}_{OO}(x_{O}-\mu_{O})=Ax_{O}+b$ for notational simplicity, where $A=\Sigma_{NO}\Sigma^{-1}_{OO}$, $b=\mu_{N}-\Sigma_{NO}\Sigma^{-1}_{OO}\mu_{O}$. We calculate the density function of $\overline{p}(x_{N},x_{O})$,

[TABLE]

Thus, the improved joint probability distribution $\overline{p}(x_{N},x_{O})$ has the following improved precision matrix and information vector,

[TABLE]

We can see from (24) that if originally $\Sigma_{NO}=0$, then $\Sigma_{N\mid O}=\Sigma_{NN}$ and $A=\Sigma_{NO}\Sigma^{-1}_{OO}=0$, thus $\overline{\Omega}$ is also block diagonal, and the improvement of $\overline{\Omega}_{OO}$ and $\overline{\xi}_{O}$ does not affect the distribution of $x_{N}$.

5. Numerical experiment

We performed numerical experiments for the planar robot system (3), (4), where the robot is assumed to be round with diameter $0.5\,\rm{m}$, and binocular cameras are equipped on the left and right sides of the robot to detect feature points, each with a field angle of $120^{\circ}$. The odometry measures local velocity $v$ (units: m/s) and angular velocity $w$ (units: rad) with Gaussian noise $\mathcal{N}(\textbf{0},Q_{v})$, $\mathcal{N}(0,Q_{w})$, respectively. We set $Q_{v}=\begin{pmatrix}0.01&0\\ 0&0.01\end{pmatrix}$ and $Q_{w}=0.01$ during the simulation. The camera is assumed to measure the relative direction of the feature point with Gaussian noise $\mathcal{N}(0,Q_{z})$. As we mentioned in Section 2, at each step, the measurement of feature points by the binocular cameras shall be combined together to give an estimate of the relative position of the feature points, and the uncertainty in the relative position is assumed to be described by an approximate Gaussian distribution. The odometry and camera readings are updated at the same frequency.

We tested three different methods, the first is the classical EKF on the state space $(p,\theta,p^{f}_{1},p^{f}_{2}\dots p^{f}_{N})$, with the propagation equation (9); the second is the First Estimate EKF (FE-EKF) where the First Estimate technique in [4] is used; the third is our reduced EKF on the state space $(p,\theta,z_{1},z_{2}\dots z_{N})$, with propagation equation (18) and reduced measurement update equation (24). We observed superior performance of the reduced EKF compared to classical EKF and FE-EKF, especially at estimating the robot’s orientation $\theta$. Furthermore, the reduced EKF is less sensitive to measurement noise.

We considered three different trajectories: straight line, circle, and a general trajectory. Feature points are generated along the trajectory randomly with a given density. We assume that there are no errors introduced during feature detection and matching. We ignore feature points that are far away (distance $>5\,\rm{m}$) from the robot, since triangulation for such points by binocular cameras are unstable.

5.1. Straight line

We tested on a straight line trajectory that is $60$ m long, and the robot traveled along it with constant speed $1\,\rm{m/s}$. We also compared low and high densities of feature points.

In Figure 1, the red line is the trajectory, the blue points are features, and yellow circle is the robot. For the low density feature distribution, the average feature points recognized at each step is approximately 20, and for the high density feature distribution, the average recognized feature points at each step is approximately 40. Besides the feature distribution, we also considered update frequencies of $10\,\rm{Hz}$ and $20\,\rm{Hz}$. Moreover, for fixed feature density and update frequency, we adjusted $Q_{z}$ to observe the sensitivity of the different methods to changes in the measurement noise. Since the feature distribution is generated randomly for fixed feature density, and odometry and camera readings are also generated randomly from the given parameters, the results for a given method will vary between realizations. The three methods (EKF, FE-EKF, reduced EKF) are applied to the same data set of feature distribution and sensor readings, and we generate 20 realizations per combination of update frequency, feature distribution, and measurement noise. We summarize the average position error (units: meters) and the orientation error (units: rad) in Table 1 and Table 2.

We see that the accuracy of all three methods improve a little bit when we double the update frequency and increase the average feature number per step, which is quite natural. In addition, when we increase the uncertainty of measurement $Q_{z}$, the performance of all three methods degrade. There is no evident improvement in accuracy for both FE-EKF and reduced EKF methods over the classical EKF method for the straight line case. A typical error growth in the position and orientation for the straight line trajectory is given in Figure 2.

5.2. Circle

The robot now travels along a circle of radius $10\,\rm{m}$, with constant speed that is approximately $1.57\,\rm{m/s}$. This is illustrated in Figure 3. In contrast to the straight line case, we observed superior performance of the reduced EKF method over both the classical EKF and FE-EKF methods.

For the circle trajectory with low feature density, the average feature number per step is approximately 20, and for the high feature density, the average feature number per step is approximately 40. The average position error and orientation error of all three methods are summarized in Table 3 and Table 4. The improvement of reduced EKF method is quite significant compared to the classical EKF and FE-EKF methods. It can reduce the orientation error by approximately $50\%$ in each case, and as a consequence, the estimation of position is also improved, and the position error is reduced by approximately $30\%$ to $50\%$. A typical error growth for the circle trajectory is given in Figure 4.

5.3. General trajectory

Besides the straight line and circle trajectories, we also consider a general trajectory, see Figure 5. The average feature number for the low feature density per step is approximately 25, and the average feature number for the high feature density per step is approximately 50.

As before, we tested all three methods on this general trajectory, and summarized their average position error and orientation error in Table 5 and Table 6. When the update frequency is $20\,\rm{Hz}$, we observe noticeable improvement in both the position error and orientation error by the reduced EKF method. The FE-EKF method exhibits better performance than the classical EKF, but the reduced EKF method works even better. When the update frequence is $10\,\rm{Hz}$, and the measurement noise $Q_{z}=1\times 10^{-4}$, we see that the error of the reduced EKF is actually larger, which is not observed in the circle case. This might possibly be due to the high velocity of the general trajectory, since we construct the trajectory by cubic spline interpolation, and the velocity is very high around corners, which is unrealistic in practice as the actuation bounds in a robot result in bounded velocities as well.

Thus, the propagation error is large when the update frequency is low, and it seems that when the discrete system deviates far away from the true trajectory, forcing it to obey the observability property of the underlying continuous system actually degrades the performance of the algorithm. This is perhaps understandable, as when the timestep is too large to accurately integrate the trajectory, the unobservable subspace becomes poorly approximated as well. This is easily resolved by doubling the update frequency. Also, we see that even when the update frequency is $10\,\rm{Hz}$, when we increase the measurement noise to $4\times 10^{-4}$, the reduced EKF method maintains a rather robust orientation error, and its position error is now smaller than the classical EKF and FE-EKF methods, which demonstrates the stability of the reduced EKF method to large measurement noise. This can be understood in terms of the relative error from measurement noise compared to the propagation step. When the measurement noise is larger, the spurious information introduced to the unobservable variables will be significant, which degrades the estimation accuracy. We believe that the reduced EKF method that respects the observability constraint is best suitable for applications where the propagation step is more precise than the measurement step. This is the case in typical VIO systems, where the IMU for propagation has very high update frequency ($200$–$500\,\rm{Hz}$), and is very accurate for short time estimation, and the camera for measurement has low update frequency ($10$–$30\,\rm{Hz}$). A typical error growth in the general trajectory case is given in Figure 6.

6. Conclusion and Future Research

We consider the observability properties of a control system in the context of Lie group symmetries. Due to the Lie group symmetry, the state variable can be decomposed into the unobservable and observable parts $(x_{N},x_{O})$ explicitly. Since the Lie group action on $x_{N}$ is trivial, the problem of requiring that the propagation step of the Kalman filter satisfies the geometric property that the unobservable subspace remains perpendicular to the measurement $dh$ is naturally formulated in terms of the reduced control system on the symmetry reduced space corresponding to the observable variables. Moreover, in order to deal with the problem that this property is destroyed at the measurement update step of the Kalman filter, we propose a reduced Bayesian inference method, i.e., only the observable part $x_{O}$ is updated by the reduced measurement. This special procedure guarantees that the unobservable variable $x_{N}$ remains unobservable, and no spurious information is introduced by the measurement update. We applied this approach to the problem of the planar robot equipped with odometry sensors and cameras, and the reduced Kalman filter method based on observability considerations outperforms the traditional Kalman filter as well as the FEJ-Kalman filter by quite a lot.

In the future, we intend to apply this technique to a practical VIO system, where the geometry involved is more complicated. In particular, the rotation group in $\mathbb{R}^{3}$ is $SO(3)$ which is nonabelian, and only rotations about the gravity direction are unobservable. Also, we would like to update the multi-state constraint Kalman filter (MSCKF) framework, where the state variables include historic poses of the camera, which makes the decomposition into observable and unobservable components more interesting. In principle, all traditional techniques that are applicable to the Kalman filter, such as UKF, particle filters, and so on, could all be combined with our reduced update method based on the observability property, and we would like to test the efficiency of our method for all related practical applications.

Acknowledgements

This research has been supported in part by NSF under grants DMS-1010687, CMMI-1029445, DMS-1065972, CMMI-1334759, DMS-1411792, DMS-1345013, DMS-1813635, and by AFOSR under grant FA9550-18-1-0288.