Commit a9810838 authored by Justin Cano's avatar Justin Cano
Browse files

updated readme and doc

parent 7331b146
No preview for this file type
Aim of our work
===============
# Description
We propose a simple EKF implementation for a passive ranging problem
requiring clock drift estimator. The drift of the clock is denoted as
$\delta(t) = t^{tag} - t^\mathcal{S}$ where $t^\mathcal{S}$ is
representing the synchronized time of the base stations and $t^{tag}$
the time in the passive tag (it is allowed only to receive messages).
Thus we will estimate a state containing $\delta$ it’s time derivative
$\dot{\delta}= \gamma$, the Cartesian coordinate of the tag in the
global frame and their associate velocities. To simplify our model, we
consider accelerations as Gaussian perturbations.
This repository contains an implementation in `C` for embedded micro-controllers of an Extended Kalman Filter for UGV navigation. Measurement used are synchronous UWB ranging message performing PTP synchronisation between agents using TOA protocol. For further details, please read the documentation PDF and the companion article.
Mathematical formulation of the problem
=======================================
## Documentation
Dynamic model
-------------
All the documentation is contained inside `application_notes.pdf`at the root of this repository.
The continuous model is defined as follows :
$$\dot{{\mathbf}x} = {\mathbf}A {\mathbf}x + {\mathbf}G \pmb \omega.$$
With the state vector and the transition matrix equals to :
$${\mathbf}x
=
\begin{bmatrix}
x \\
vx \\
y \\
vy \\
z \\
vz \\
\delta \\
\gamma
\end{bmatrix},
\quad
{\mathbf}A =
\begin{bmatrix}
0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \\
0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
\end{bmatrix}
=
\begin{bmatrix}
{\mathbf}S_2 & {\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}0_2 \\
{\mathbf}0_2 & {\mathbf}S_2 & {\mathbf}0_2 & {\mathbf}0_2 \\
{\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}S_2 & {\mathbf}0_2 \\
{\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}S_2 \\
\end{bmatrix}
.$$ And thus, for the process noise, we have : $$\pmb \omega =
\begin{bmatrix}
\pmb \omega_{vx} \\
\pmb \omega_{vy} \\
\pmb \omega_{vz} \\
\pmb \omega_{\delta} \\
\pmb \omega_{\gamma}
\end{bmatrix}
\text{ and }
\pmb G =
\begin{bmatrix}
0 & 0 & 0 & 0 & 0 \\
1 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 \\
0 & 0 & 1 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 \\
0 & 0 & 0 & 0 & 1 \\
\end{bmatrix}.$$
## Contributor
We want to discretize on $h_k$ this model, so we have :
$${\mathbf}x_k = \pmb \Phi(h_k) {\mathbf}x_{k-1} + \pmb \omega_k$$ So we
can apply the exponential function to ${\mathbf}A$ :
$$\pmb \Phi := {\mathbf}\exp({\mathbf}A h_k) = {\mathbf}I_8 + {\mathbf}A h_k.$$
Justin Cano, PhD Student, Polytechnique Montréal (QC, Canada) - ISAE Supaéro (Toulouse, France)
And we can discretize the covariance matrix of the process noise,
assuming $ \pmb \omega \sim \mathcal{N}({\mathbf}0, {\mathbf}\Sigma_c)$
with
${\mathbf}\Sigma_c = \textbf{diag}(\sigma_x^2,\sigma_y^2,\sigma_z^2,\sigma_\delta^2,\sigma_\gamma^2)$,
we get :
Reference
---------------
$$\pmb \omega_k \sim \mathcal{N}({\mathbf}0, {\mathbf}Q) \text{ with }
{\mathbf}Q =
\begin{bmatrix}
{\mathbf}W \sigma^2_x & {\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}0_2 \\
{\mathbf}0_2 & {\mathbf}W \sigma^2_y & {\mathbf}0_2 & {\mathbf}0_2 \\
{\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}W \sigma^2_z & {\mathbf}0_2 \\
{\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}0_2 & {\mathbf}W \sigma_\gamma^2 + {\mathbf}S_2 \sigma^2_\delta \\
\end{bmatrix},
\text{ and }
{\mathbf}W =
\begin{bmatrix}
\frac{T^3}{3} & \frac{T^2}{2}\\
\frac{T^2}{2} & T
\end{bmatrix}.$$
Measurement model
-----------------
We get scalar measurements of two types : skew measurement and
pseudo-range ones. So we have two functions linking the measurement to
the previous state space : $$\begin{aligned}
\tilde{y}_\gamma({\mathbf}X) &= \gamma + \nu_\gamma \text{ with } \nu_\gamma \sim \mathcal{N}(0,\sigma_{\gamma m}^2) \\
\tilde{y}_\rho^i({\mathbf}X) &= \underbrace{\sqrt{(x-x_i)^2+(y-y_i)^2+(z-z_i)^2}}_{d_i} + c \delta + \nu_\rho \text{ with } \nu_\rho \sim \mathcal{N}(0,\sigma_{\rho}^2).\end{aligned}$$
Therefore we are able to calculate the two corresponding Jacobians for
projected estimates :
$${\mathbf}J_l({\mathbf}X) = \frac{\partial y_l({\mathbf}X)}{\partial {\mathbf}X} =
\begin{cases}
\begin{bmatrix}
0 & 0 & 0 & 0 & 0 & 1
\end{bmatrix}
\text{ if } l=\gamma, \\ \\
\begin{bmatrix}
\frac{x-x_i}{d_i} & 0 & \frac{y-y_i}{d_i} & 0 & \frac{z-z_i}{d_i} & c & 0
\end{bmatrix}
\text{ if } k=\rho
\end{cases}.$$
Extended Kalman Filter
----------------------
Thus we use the EKF algorithm as follows [^1] :
1. Prediction steps (estimate and covariance) :
$$\hat{{\mathbf}x}_{k|k-1} = \pmb \phi_k \hat{{\mathbf}x_{k-1}}, \quad {\mathbf}P_{k|k-1} = {\mathbf}A {\mathbf}P_{k-1} {\mathbf}A^t + {\mathbf}Q.$$
2. Jacobian ${\mathbf}J_k$ computation.
3. Innovation step (scalar and covariance) :
$$i_k = \tilde{\rho}_l^i - \rho^i(\hat{{\mathbf}X}_{k|k-1}), \quad s_k = {\mathbf}J_k {\mathbf}P_{k|k-1} {\mathbf}J_k^t + \sigma_{lm}^2.$$
4. Kalman gain computation :
$${\mathbf}g_k = {\mathbf}P_{k|k-1} {\mathbf}J_k^T s_k^{-1}.$$
5. Update step (estimate and covariance) :
$$\hat{{\mathbf}x} = \hat{{\mathbf}x}_{k|k-1} + {\mathbf}g_k i_k, \quad {\mathbf}P_k = ({\mathbf}I_8 - {\mathbf}g_j {\mathbf}J_k){\mathbf}P_{k|k-1}.$$
We can initialize $\hat{{\mathbf}x}_0 = {\mathbf}0$ and
${\mathbf}P_0 = 100 {\mathbf}Q$ for example (weak information on drift
nor in initial position and thus a strong uncertainty).
Implementation
==============
The implementation is done in `C` language
Matricial features
------------------
Findable in the `matrix.c/h` file set.
### Setting your arithmetical precision
You can choose your arithmetical precision by setting the file
`precision_floating.h` using an alias `fl` for all floating point
variables. If you change the alias thus you change the precision of your
calculus.
### Simple operations
#### Matrix struct :
A matrix is composed of two 16-bits integers representing its size.
Moreover a buffer, is representing the `fl` type coefficient of your
matrix. We have to allocate memory to this buffer before following
further calculus. When you declare a matrix you must call the allocation
process.
#### Allocation :
We can allocate memory to the matrix buffer by simply calling the
function `createMatrix(uint16_t n_rows,uint16_t n_columns)`. It has to
be done for each matrix.
#### Reset a matrix :
By reset, I mean set to zero each coefficient, this is feasible thanks
to `mzeros(matrix M)`.
##### *Caution :*
For each the following functions, the first argument is the return
value. Be aware that the correct way to compute the results are to use
three (resp. two) different matrix structures. For example
${\mathbf}A = {\mathbf}A {\mathbf}B$ has to be implemented such as :
`mmul(tempA,A,B` where `tempA` is a temporary structure to store the
result.
#### Unary operations :
The transposition (`mtrsp`), the display (`mprint`) and display with
Matlab format is allowed by our library. **Caution : display is not
compliant with some embedded board, only to debug on your computer in
simulation then.**
#### Binary operations :
The `mcopy` operation is equivalent to the matricial equality
(${\mathbf}A = {\mathbf}B$), we have also matrix multiplication
(`mmul`), matrix addition and subtraction (`madd,msub`). The division is
to be done (not required here because our EKF inversion is a scalar one.
We have also the dot product between two vectors of the same dimension
(`mscal`) and finally the `midcomp(matrix A,matrix B)` operation
performing ${\mathbf}A = {\mathbf}I - {\mathbf}B$. A random (`mrandom`)
matrix generator, following uniform law between -100 and 100 is also
present.
The filter algo itself
----------------------
It’s findable in `kalman_localization.h/c` and two functions have to be
call outside it (in your custom high-level application). The filter has
to be initialized with the *ekf\_init* function and routine has to be
call trough `ekf_loop(i)` where $i$ represent the anchor number. If
$i=255$ is it assumed that our measurement is a skew ($\tilde{\gamma}$)
measurement coming from the master station.
Simulation features
-------------------
A simulation of our filter is directly implemented in `test.c` and it’s
compilable with a make command using gcc compilatior (the makefile is
ready to be used). We found that it was a good way to show how our code
is working. The next section of this document is dedicated to it.
An exemple : Circular trajectory tracking
=========================================
Trajectory and noise generation
-------------------------------
We assume that the robot (thus $z=0,v_z=0$) is describing a circular
trajectory within ten seconds following the unitary circle. Thus we have
the simulated position states such as : $$\begin{cases}
x_k = r \cos(t_k \omega), \\
y_k = r \sin(t_k \omega)
\end{cases}.$$ We have $t_{k+1} = t_k + h_k$ and assuming that our
operating frequency is $80 Hz$ we can write $h_k = 1/80$. Thus
$\omega = 2 \pi / 10$ (a complete circle in 10 s). For the velocities,
we get : $$\begin{cases}
vx_k = -\omega r \sin(t_k \omega), \\
vy_k = \omega r \cos(t_k \omega)
\end{cases}.$$ We assume that our drift states are a constant[^2] skew
$\gamma_k = -2 ppm$ and thus we can compute a linear offset following
the equation $\delta_{k+1} = \delta_k + h_k \gamma_k$ and with
$\gamma_0 = 10^{-2}s$.\
We have also implemented Gaussian centred noise generator trough `normc`
function. And thus, we can compute perturbed pseudorange such as :
­$$\tilde{\rho}_k^i =
\begin{vmatrix}
\begin{vmatrix}
\begin{bmatrix}
x_k \\
y_k \\
z_k
\end{bmatrix}
-
\begin{bmatrix}
x_i \\
y_i \\
z_i
\end{bmatrix}
\end{vmatrix}
\end{vmatrix}
_2
+ c \delta + \nu_\rho, \quad \nu_\rho \sim \mathcal{N}(0,0.1^2).$$
We will do our estimation only with pseudorange estimates, with a
variance of .1 meters. For this simulation, only 4 base stations are
set.
Datalog
-------
After runing the algorithm two datalog files are provided :
- `vehicle_data.csv` containing states and estimates of the positions
and drifts of the vehicle’s tag.
- `measurements.csv` containing raw data (pseudoranges).
Ploting demo with our `Python` script and usage
-----------------------------------------------
Matplotlib has to be installed alongside Python to plot features. But
Matlab and even Excel can fit to import CSVs ;)\
We can first compile our C code with the `make` command (invoking GCC in
Linux systems, makefile to adapt if windows). Then, the program
`kalman_demo` will be updated and executable with the command
`./kalman_demo`. The datalog will be soon updated and ready to be print.
If some permissions issues appears in the execution process you can make
executable the program by typing `chmod +x kalman_demo`.\
After that you can execute our custom script by typing `python plot.py`
in the command prompt. Three graphs representing positions, velocities
and drift states, will normally appears and be saved as `res/pos.png`,
`res/vel.png` and `res/drift.png`. We have yet provided in this repo the
figures.\
Note that an equivalent script in Matlab is provided in this repo using
pre-recorded data and useful for debug.\
If you have some issues with the code you can contact the author through
Gitlab platform.
Reference {#reference .unnumbered}
=========
This implementation comes alongside this article published in ICRA 2019
This implementation comes alongside the following paper published in ICRA 2019
proceedings :
J. Cano, S. Chidami, and Le Ny, Jérôme, “A Kalman Filter-Based Algorithm
for Simultaneous Time Synchronization and Localization in UWB Networks,”
in ICRA, Montreal, QC, Canada, 2019.
[^1]: Formulation adapted from
<https://en.wikipedia.org/wiki/Extended_Kalman_filter> .
[^2]: It’s evolving very slowly.
Article preprint is available on http://justincano.com
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment