Computing mean and covariance of samples that contain angular states

95 Views Asked by At

I'm trying to compute the mean and covariance of a population of samples, but I'm struggling to figure out how to deal with angular states in each sample. The state vector of each sample is given by,

$$s = (x, y, \theta)$$

where $x$ and $y$ are the sample's x-position and y-position, respectively, in a 2D Euclidean space, and $\theta$ is its angular heading in degrees or radians (e.g. this could represent the pose of a simple wheeled robot moving in a "flat" world/map frame).

The standard way of computing the mean and covariance of a population is,

$$\mu = \frac{1}{N}\sum_{i=1}^N s_i$$

$$\Sigma = \frac{1}{N}\sum_{i=1}^N (s_i - \mu)(s_i - \mu)^T$$

where $N$ is the number of samples. However, since common operations such as addition and multiplication aren't as straightforward for angular values as they are for Euclidean values, the $\theta$ state in each sample has been causing some issues when trying to compute the above mentioned mean and covariance.

I was able to solve the problem of computing the mean of $\theta$ using this useful webpage, where we have,

$$\mu = \operatorname{atan2} \left(\sum_{i=1}^N \sin\theta_i, \sum_{i=1}^N \cos\theta_i \right)$$

but have not had much luck finding a way to compute the covariance. As such, I was wondering if anyone could help me understand how I can compute the covariance of a population, when one of its states is an angular quantity. Any help or links to explanations would be appreciated.