|
[DPRG] Simple UKF example in 2D
Subject: [DPRG] Simple UKF example in 2D
From: Chris Jang
christopher.jang at yahoo.com
Date: Wed Dec 5 01:38:39 CST 2007
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
More information about the DPRG mailing list
|
