DPRG mailing list archives: [DPRG] Simple UKF example in 2D

Wed Dec 5 09:15:03 CST 2007

I just noticed a mistake in how the sigma points are
used to calculate the mean and the covariance. The
corrected code for this follows. The sigma point
weights were not being used correctly.

This UKF example is significantly more complex than
the EKF case. I don't know if this changes with
different problems. I do know that sometimes a EKF and
UKF perform about the same. The impression I have is
the the UKF does everything the EKF does and also
handles more (when system becomes too non-linear).

I had thought that if the KF could be formulated in
terms of linear algebra, then I wouldn't have to know
statistics. It did not work out this way. The
conceptual framework of "prior estimate", "posterior
estimate", etc is the most natural way of thinking. So
even though I set out to learn about the KF in terms
of math I already knew, over time I started to think
in terms of statistics just because this was easier. I
think a big part of what makes the KF difficult is
that knowledge of both linear algebra and statistics
really helps. This combination was unusual when I was
in school as statistics was a separate
subspecialization within the math department.

    // sigma point mean and covariance of: dt *
cos(th)
    double avg_x = 0, avg_y = 0;
    double cov_xx = 0, cov_yy = 0, cov_xy = 0;
    for (int i = 0; i < 5; i++)
    {
        double xtmp = (dt + _dt[i]) * cos(th +
_th[i]);
        double ytmp = (dt + _dt[i]) * sin(th +
_th[i]);
        double a    = a[i];
        avg_x += a * xtmp;
        avg_y += a * ytmp;
        cov_xx += a * xtmp * xtmp;
        cov_yy += a * ytmp * ytmp;
        cov_xy += a * xtmp * ytmp;
    }
    cov_xx -= avg_x * avg_x;
    cov_yy -= avg_y * avg_y;
    cov_xy -= avg_x * avg_y;

--- Chris Jang <christopher.jang at yahoo.com> wrote:

> I wanted to try the same problem as in:
> 
>
http://list.dprg.org/archive/2007-November/031051.html
> 
> with an unscented Kalman filter. The only difference
> is that noise is characterized by 5 sigma points
> through the non-linear odometry update:
> 
> x(k+1) = x(k) + distance * cos(theta)
> y(k+1) = y(k) + distance * sin(theta)
> 
> Variance in distance and theta become covariance in
> x
> and y after the update. The UKF more accurately
> characterizes the uncertainty by sampling. In the
> EKF
> example, we just chose sufficiently large variance
> values in x and y and ignored the polar coordinate
> system. This is the main difference between the two
> examples.
> 
> 
> 
> 
> double x, y;    // current best estimate of position
> double x_, y_;  // prediction using odometry process
> 
> double dt;      // odometry distance traveled
> double th;      // odometry bearing
> 
> double zx, zy;  // GPS position measurement
> 
> double pxx, pyy, pxy;  // covariance in position
> estimate
> double wdt, wth;       // variance in odometry
> process
> double vx, vy;         // variance in GPS
> measurement
> 
> double alpha;   // sigma-point tuning factor
> 
> // sigma point weights
> double a[5];
> a[0] = (alpha * alpha - 1) / alpha * alpha;
> a[1] = a[2] = a[3] = a[4] = 1 / (4 * alpha * alpha);
> 
> // sigma points are around the mean
> double _dt[5], _th[5];
> _dt[0] = _th[0] = 0
> _dt[1] = alpha * sqrt(2) * sqrt(wdt);
> _dt[2] =  _dt[1];
> _dt[3] = -_dt[1];
> _dt[4] = -_dt[1];
> _th[1] = alpha * sqrt(2) * sqrt(wth);
> _th[2] = -_th[1];
> _th[3] =  _th[1];
> _th[4] = -_th[1];
> 
> while (1)
> {
>     zx = gpsX();
>     zy = gpsY();
>     dt = odoDistance();
>     th = odoTheta();
> 
>     // prior estimate, uses odometry process model
>     x_ = x + dt * cos(th);
>     y_ = y + dt * sin(th);
> 
>     // sigma point mean and covariance of: dt *
> cos(th)
>     double avg_x = 0, avg_y = 0;
>     double cov_xx = 0, cov_yy = 0, cov_xy = 0;
>     for (int i = 0; i < 5; i++)
>     {
>         double xtmp = a[i] * (dt + _dt[i]) * cos(th
> +
> _th[i]);
>         double ytmp = a[i] * (dt + _dt[i]) * sin(th
> +
> _th[i]);
>         avg_x += xtmp;
>         avg_y += ytmp;
>         cov_xx += xtmp * xtmp;
>         cov_yy += ytmp * ytmp;
>         cov_xy += xtmp * ytmp;
>     }
>     avg_x  = avg_x / 5;
>     avg_y  = avg_y / 5;
>     cov_xx = (cov_xx / 5) - avg_x * avg_x;
>     cov_yy = (cov_yy / 5) - avg_y * avg_y;
>     cov_xy = (cov_xy / 5) - avg_x * avg_y;
> 
>     // covariance in prediction (prior estimate)
>     double pxx_ = pxx + cov_xx;
>     double pyy_ = pyy + cov_yy;
>     double pxy_ = pxy + cov_xy;
> 
>     // Kalman gain matrix
>     double tmp = pxx_ * pyy_ - pxy_ * pxy_;
>     double K11 = 1 + (pxx_ * vy) / tmp;
>     double K22 = 1 + (pyy_ * vx) / tmp;
>     double K21 = (pxy_ * vy) / tmp;
>     double K12 = (pxy_ * vx) / tmp;
> 
>     // posterior estimate, Kalman gain and
> measurement
> correction
>     x = x_ + K11 * (zx - x_) + K12 * (zy - y_);
>     y = y_ + K21 * (zx - x_) + K22 * (zy - y_);
> 
>     // update covariance in posterior estimate
>     pxx = (1 - K11) * pxx_ - K12 * pxy_;
>     pyy = (1 - K22) * pyy_ - K21 * pxy_;
>     pxy = (1 - K22) * pxy_ - K21 * pxx_;
> }
> 
> 
> 
>      
>
____________________________________________________________________________________
> Be a better friend, newshound, and 
> know-it-all with Yahoo! Mobile.  Try it now. 
>
http://mobile.yahoo.com/;_ylt=Ahu06i62sR8HDtDypao8Wcj9tAcJ
> 
> 
> 
> _______________________________________________
> DPRGlist mailing list
> DPRGlist at dprg.org
> http://list.dprg.org/mailman/listinfo/dprglist
> 

      ____________________________________________________________________________________
Looking for last minute shopping deals?  
Find them fast with Yahoo! Search.  http://tools.search.yahoo.com/newsearch/category.php?category=shopping