Consensus estimation of rigid body motion using lie algebra se(3) and the consensus extended kalman filter

In the framework of geometric mechanics, the consensus extended Kalman flter is em-ployed for the problem of estimation of rigid body motion using position measurements from feature points. The position and attitude (pose) of the rigid body is represented by Lie algebra se(3). In the proposed strategy multiple sensors in a communication network corporately estimate the motion of the rigid body by using the consensus extended Kalman flter. Numerical results demonstrate that the flter can converge on the true state when measurements of three feature points are processed by a single Kalman flter. When multiple flters are allowed to communicate their local estimates with each other within a connected communication network, the fltering performance (estimation error and covari-ance envelope) is greatly improved and the number of feature points required to guarantee the observability can be relaxed.