9
$\begingroup$

My team is building a robot to navigate autonomously in an outdoor environment. We recently got a new integrated IMU/GPS sensor which apparently does some extended Kalman filtering on-chip. It gives pitch, roll, and yaw, north, east, and down velocities, and latitude and longitude.

However, we also have some encoders attached to our wheels, which provide linear and angular velocities. Before we got this new IMU/GPS sensor, we made our own EKF to estimate our state using the encoders and some other low-cost sensors. We want to use this new sensor's on-chip filter, but also incorporate our encoders into the mix.

Is there any problem with chaining the filters? What I mean is, we'd use the output of the IMU/GPS sensor's on-chip EKF as an update to our own EKF, just as we use the data read from the encoders as an update to our EKF. It seems reasonable to me, but I was wondering what is usually supposed to be done in this case.

$\endgroup$
  • $\begingroup$ Can you feed back corrections/estimates into the EKF "black box"? $\endgroup$ – Damien Mar 1 '13 at 4:51
  • $\begingroup$ @Damien no, there doesn't seem to be a way to do that. $\endgroup$ – Robz Mar 4 '13 at 9:57
4
$\begingroup$

You can use the INS / GPS as updates to the output of your first EKF. This is, in fact, not chaining, but simply conditioning the estimate based on the added information from the INS / GPS.

Suppose we have the following functions:

$x_{t+1|t}$, $P_{t+1|t}$ = EKF_PREDICT($x_t$, $P_t$, $u_t$), for inputs as state $x$, covariance $P$, and control inputs (estimated by odometry) $u_t$.

and

$x_{t+1|t+1}$, $P_{t+1|t+1}$ = EKF_UPDATE($x_{t+1|t}$, $P_{t+1|t}$, $\hat{x}_{t+1}$).

The estimates from sensors are the $\hat{x}_{t+1}$. We have things like:

$\hat{x}^{gps}_{t+1} = f(GPS)$

$\hat{x}^{map}_{t+1} = f(map)$

$\hat{x}^{ins}_{t+1} = f(INS)$

etc for all other ways of estimating the state of the robot. So running the function EKF_UPDATE for all of those sensors is good enough.

Your loop will be something like this:

for all time $t$

  • Let $u_t$ be the current odometry / kinematic estimate of pose, and $R_u$ be the noise on that estimate.

  • $x_{t+1|t}$, $P_{t+1|t}$ = EKF_PREDICT($x_t$, $P_t$, $u_t$, $R_u$)

  • for all sensors $S$,

    • Let $\hat{x}^{S}_{t+1}$ be the estimate of the pose from that sensor, and $R_{S}$ be the noise on that estimate

    • $x_{t+1|t+1}$, $P_{t+1|t+1}$ = EKF_UPDATE($x_{t+1|t}$, $P_{t+1|t}$, $\hat{x}_{t+1}, R_S$).

    • end-for

  • end-for

Some caveats are:

  • Since we're using the EKF, there is no guarantee that the estimate is independent of the ordering of the updates. That is, if you do INS then GPS, the resulting estimate might be different than if you update with GPS then INS. This is usally not a big deal, but the filter will requiring significantly more tuning.

  • Please be aware your INS has a bias and drift, which might affect your long-term reliability. GPS can help you a lot here. Most literature simultaneously estimates the bias and drift in the INS.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.