Background
I have a car-like mobile robot (4 wheels, where the forward ones are steering wheels) and I want to estimate its pose and velocity assuming 2D planar motion. I'm trying to solve this problem by setting up an EKF.
The state vector that I want to estimate is composed as:
$$
\textbf{x} =
\begin{bmatrix}
x \\
y \\
\theta \\
v \\
\omega
\end{bmatrix}
$$
Where:
- $x, y$ and $\theta$ is the pose of the vehicle and it is expressed in a global frame named map
- $v$ and $\omega$ are the linear and angular velocity of the vehicle expressed in a local frame attached to it named base link
The sensors that I have on this vehicle are the following:
- Velocity Encoders (rear wheels) + Steering angle (forward wheels): these data come directly from the low level hardware of the vehicle. I want to use these data as input in the propagation step of the filter by using a bicycle kinematic model as motion model.
- IMU providing linear acceleration $a^I_k$ and angular velocity $\omega^I_k$ in the base link frame
- GPS receiver which provides lat and lon coordinates of the vehicle. The lat and lon will be preprocessed and converted in the map frame, thus providing $x_G$ and $y_G$ position information.
EKF Formulation
Propagation Step
As previously said, I want to use a bicycle kinematic model and the "vehicle feedback data" (encoders + steering angle) to perform the state propagation step.
From wheel encoders I can easily retrieve vehicle linear velocity, thus the input of the bicycle model will be $u_k = [v^u_k, \delta^u_k]^T$.
We have also to take into account uncertainty in the data provided by the vehicle and we model this uncertainty as a zero mean gaussian noise acting on both the terms of the input $u_t$:
$$ u_k = u^r_k + w_k $$ where $u^r_t$ is the real input and $w_t$ a gaussian noise term with zero mean and covariance matrix $Q_k$: $$ Q_k = \begin{bmatrix} \sigma^2_v & 0 \\ 0 & \sigma^2_\delta \end{bmatrix} $$
Then we just need to linearize and compute Jacobians $F_k$, $L_k$ and we are ready to propagate the state every time a new vehicle feedback data arrives by using the bicycle model. The state prediction step involves also the propagation of the uncertainty by increasing the state covariance matrix.
The predicted state and covariance at time $k$ are thus obtained as following:
$$ x^p_k = f_{bicycle}(x_{k-1}, u_k) $$ $$ \Sigma^p_k = F_{k} * \Sigma_{k-1} * F^T_{k} + L_{k} * Q_k * L^T_{k} $$
Up to now it is everything fine and the filter works as expected, by estimating the vehicle state and propagating the uncertainty as new data arrive.
IMU Correction Step
Here is where problems start arising.
From IMU I get vehicle linear acceleration $a^I_k$ and angular speed $\omega^I_k$. My idea is to integrate these data to retrieve also an "indirect" measurement of the other state components by using standard motion laws:
$$ x^I_k = x^I_{k-1} + (v^I_{k-1} \Delta_k + \dfrac{1}{2} a^I_{k} \Delta^2_k) \cdot cos(\theta^I_{k-1}) \\ y^I_k = y^I_{k-1} + (v^I_{k-1} \Delta_k + \dfrac{1}{2} a^I_{k} \Delta^2_k) \cdot sin(\theta^I_{k-1}) \\ \theta^I_k = \theta^I_{k-1} + w^I_k \Delta_k \\ v^I_k = v^I_{k-1} + a^I_{k} \Delta_k \\ \omega^I_k = \omega^I_k $$ I will use $f^I_k(\cdot)$ to refer to these system of equations.
Please notice that:
- $f^I_k(\cdot)$ can be seen as a state propagation function stored internally by the IMU subsystem
- this propagation is done with respect to the previous "IMU" state and not to the state vector $x_k$ of the EKF (I do this to prevent the IMU indirect measurements to get biased by the filter)
All these things mean that the IMU subsystem will provide to the EKF a (indirect) measurement obtained by an integration of the IMU state performed internally by the IMU subsystem.
In conclusion, the IMU measurement model for the EKF results to be:
$$ z_k = x^I_k = f^I_k(x^I_{k-1},I_k) = h(x^p_k) $$
Where $h(x^p_k)$ is the usual function that "maps" the predicted state to the measured quantity needed to compute the innovation term of EKF. For this case $h(x^p_k)$ is simply an identity function since from the IMU we get (directly or indirectly) the state variables.
Clearly I also want to take into account the uncertainty of the IMU input data $I_k = [a^I_k, \omega^I_k]$ by including a zero mean gaussian noise:
$$ I_k = I^r_k + v_k $$
where the covariance matrix of the IMU noise $v_k$ is named $R_k$ and defined as:
$$ R_k = \begin{bmatrix} \sigma^2_a & 0 \\ 0 & \sigma^2_\omega \end{bmatrix} $$
Now we are ready to compute the Jacobians of the measurement model. Generally this means to compute $H_k = \frac{\partial h}{\partial x_k}$ and $M_k = \frac{\partial h}{\partial v_k}$.
In our case, $H_k$ will be computed as expected and will result to be a 5x5 identity matrix.
A different reasoning must be carried out for $M_k$, which is the Jacobian expressing how the uncertainty affects the measured quantity. In my opinion, since the IMU measurement is obtained by integrating the IMU input it is correct to integrate also the noise $v_k$ acting on the IMU input $I_k$. This means that we need to compute a sort of covariance of the "IMU state" $x^I_k$ from the IMU propagation function $f^I_k(\cdot)$, and we will call this covariance $\Sigma^I_k$.
We can do this as we did in the propagation step, i.e., we compute Jacobians of $f^I_k(\cdot)$ with respect to $x_k$ and to the noise $v_k$:
$$ F^I_k = \frac{\partial f^I_k}{\partial x_k} \\ L^I_k = \frac{\partial f^I_k}{\partial v_k} $$
In conclusion, in the internal IMU subsystem we will maintain a sort of EKF propagation step made as usual:
$$ x^I_k = f^I_k(x^I_{k-1}, I_k) = z_k $$ $$ \Sigma^I_k = F^I_{k} * \Sigma^I_{k-1} * (F^I_{k})^T + L^I_{k} * R_k * (L^I_{k})^T $$
Finally, we are ready to perform the classic Correction Step of the EKF:
$$ K_k = \Sigma^p_k * H^T_k * (H_k * \Sigma^p_k * H^T_k + \Sigma^I_k)^{-1} \\ x_k = x^p_k + K_k * (x^I_k - h(x^p_k)) \\ \Sigma_k = (1 - K_k * H_k) * \Sigma^p_k $$
My first question is thus to know whether my IMU correction step approach is correct or not.
The second important question that I have is: how to handle the fact that the "IMU state" covariance $\Sigma^I_k$ grows indefinitely? Is this right as it is?
I mean, I would expect that the EKF filter made of only Encoder+steering and IMU will drift over time and thus its associated covariance $\Sigma_k$ gets bigger and bigger, but letting the IMU covariance grows indefinitely would mean to let the IMU correction to have less weight as the time increases. And this doesn't sound good to me.
One idea to solve this could be to add a last step in the correction where I set IMU covariance to be equal to the state covariance, i.e.:
$$
\Sigma^I_k = \Sigma_k
$$
In this way, at each iteration of EKF I start with an IMU which has same covariance as the system one; then, only the added noise term provided by the bicycle propagation step and the IMU measurement step will play the difference in computing the Kalman gain (i.e., who will have more weight in the correction step).
If you have other ideas then this, please let me know.
Thanks in advance to anyone who will help me.
p.s. I know that I told that I have also gps, but since I know how to integrate it into EKF, I didn't want to include it into my question
p.p.s I know that IMU has bias, but for now it is okay for me to neglect it
p.p.p.s I had a look at this question, however it doesn't explain what I'm asking here
p.p.p.p.s sorry for the lenght of my question, but I wanted to be exhaustive and clear as much as possible
