26.Predict Radar Measurement Assignment 1
The goal of this coding quiz is to predict the mean and the covariance of a radar measurement .
To be able to build the measurement covariance matrix R , you need the values for the radar measurement uncertainty .
Task List
- transform sigma points into measurement space
- calculate mean predicted measurement
- calculate measurement covariance matrix S
/**
* Programming assignment functions:
*/
void UKF::PredictRadarMeasurement(VectorXd* z_out, MatrixXd* S_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// define spreading parameter
double lambda = 3 - n_aug;
// set vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
double weight_0 = lambda/(lambda+n_aug);
double weight = 0.5/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) {
weights(i) = weight;
}
// radar measurement noise standard deviation radius in m
double std_radr = 0.3;
// radar measurement noise standard deviation angle in rad
double std_radphi = 0.0175;
// radar measurement noise standard deviation radius change in m/s
double std_radrd = 0.1;
// create example matrix with predicted sigma points
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create matrix for sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug + 1);
// mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
// measurement covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);
/**
* Student part begin
*/
// transform sigma points into measurement space
// calculate mean predicted measurement
// calculate innovation covariance matrix S
/**
* Student part end
*/
// print result
std::cout << "z_pred: " << std::endl << z_pred << std::endl;
std::cout << "S: " << std::endl << S << std::endl;
// write result
*z_out = z_pred;
*S_out = S;
}
27.Predict Radar Measurement Assignment 1
void UKF::PredictRadarMeasurement(VectorXd* z_out, MatrixXd* S_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// define spreading parameter
double lambda = 3 - n_aug;
// set vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
double weight_0 = lambda/(lambda+n_aug);
double weight = 0.5/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) {
weights(i) = weight;
}
// radar measurement noise standard deviation radius in m
double std_radr = 0.3;
// radar measurement noise standard deviation angle in rad
double std_radphi = 0.0175;
// radar measurement noise standard deviation radius change in m/s
double std_radrd = 0.1;
// create example matrix with predicted sigma points
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create matrix for sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug + 1);
// mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
// measurement covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);
/**
* Student part begin
*/
// transform sigma points into measurement space
for (int i = 0; i < 2 * n_aug + 1; ++i) { // 2n+1 simga points
// extract values for better readability
double p_x = Xsig_pred(0,i);
double p_y = Xsig_pred(1,i);
double v = Xsig_pred(2,i);
double yaw = Xsig_pred(3,i);
double v1 = cos(yaw)*v;
double v2 = sin(yaw)*v;
// measurement model
Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y); // r
Zsig(1,i) = atan2(p_y,p_x); // phi
Zsig(2,i) = (p_x*v1 + p_y*v2) / sqrt(p_x*p_x + p_y*p_y); // r_dot
}
// mean predicted measurement
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug+1; ++i) {
z_pred = z_pred + weights(i) * Zsig.col(i);
}
// innovation covariance matrix S
S.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; ++i) { // 2n+1 simga points
for (int i = 0; i < 2 * n_aug + 1; ++i) { // 2n+1 simga points
// residual
VectorXd z_diff = Zsig.col(i) - z_pred;
// angle normalization
while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
S = S + weights(i) * z_diff * z_diff.transpose();
}
// add measurement noise covariance matrix
MatrixXd R = MatrixXd(n_z,n_z);
R << std_radr*std_radr, 0, 0,
0, std_radphi*std_radphi, 0,
0, 0,std_radrd*std_radrd;
S = S + R;
/**
* Student part end
*/
// print result
std::cout << "z_pred: " << std::endl << z_pred << std::endl;
std::cout << "S: " << std::endl << S << std::endl;
// write result
*z_out = z_pred;
*S_out = S;
}
28.UKF Update
The update state mean and the update covariance matrix .These equation are exactly as for the standard KF . The only difference here is how you calculate the Kalmen gain K . Here you need the cross -correlation matrix between the predicted sigma in the state space and the predicted sigma points in the measurement space . (你需要使用到预测空间的sigma 点和测量空间的sigma点)
29.UKF Update Assignment 1
UKF Update Assignment
You will be completing the missing code in ukf.cpp, UpdateState() function.
Task List
- calculate cross correlation matrix
- calculate Kalman gain K
- update state mean and covariance matrix
/**
* Programming assignment functions:
*/
void UKF::UpdateState(VectorXd* x_out, MatrixXd* P_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// define spreading parameter
double lambda = 3 - n_aug;
// set vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
double weight_0 = lambda/(lambda+n_aug);
double weight = 0.5/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) {
weights(i) = weight;
}
// create example matrix with predicted sigma points in state space
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create example vector for predicted state mean
VectorXd x = VectorXd(n_x);
x <<
5.93637,
1.49035,
2.20528,
0.536853,
0.353577;
// create example matrix for predicted state covariance
MatrixXd P = MatrixXd(n_x,n_x);
P <<
0.0054342, -0.002405, 0.0034157, -0.0034819, -0.00299378,
-0.002405, 0.01084, 0.001492, 0.0098018, 0.00791091,
0.0034157, 0.001492, 0.0058012, 0.00077863, 0.000792973,
-0.0034819, 0.0098018, 0.00077863, 0.011923, 0.0112491,
-0.0029937, 0.0079109, 0.00079297, 0.011249, 0.0126972;
// create example matrix with sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug + 1);
Zsig <<
6.1190, 6.2334, 6.1531, 6.1283, 6.1143, 6.1190, 6.1221, 6.1190, 6.0079, 6.0883, 6.1125, 6.1248, 6.1190, 6.1188, 6.12057,
0.24428, 0.2337, 0.27316, 0.24616, 0.24846, 0.24428, 0.24530, 0.24428, 0.25700, 0.21692, 0.24433, 0.24193, 0.24428, 0.24515, 0.245239,
2.1104, 2.2188, 2.0639, 2.187, 2.0341, 2.1061, 2.1450, 2.1092, 2.0016, 2.129, 2.0346, 2.1651, 2.1145, 2.0786, 2.11295;
// create example vector for mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
z_pred <<
6.12155,
0.245993,
2.10313;
// create example matrix for predicted measurement covariance
MatrixXd S = MatrixXd(n_z,n_z);
S <<
0.0946171, -0.000139448, 0.00407016,
-0.000139448, 0.000617548, -0.000770652,
0.00407016, -0.000770652, 0.0180917;
// create example vector for incoming radar measurement
VectorXd z = VectorXd(n_z);
z <<
5.9214, // rho in m
0.2187, // phi in rad
2.0062; // rho_dot in m/s
// create matrix for cross correlation Tc
MatrixXd Tc = MatrixXd(n_x, n_z);
/**
* Student part begin
*/
// calculate cross correlation matrix
// calculate Kalman gain K;
// update state mean and covariance matrix
/**
* Student part end
*/
// print result
std::cout << "Updated state x: " << std::endl << x << std::endl;
std::cout << "Updated state covariance P: " << std::endl << P << std::endl;
// write result
*x_out = x;
*P_out = P;
}
30.UKF Update Assignment2
/**
* Programming assignment functions:
*/
void UKF::UpdateState(VectorXd* x_out, MatrixXd* P_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// define spreading parameter
double lambda = 3 - n_aug;
// set vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
double weight_0 = lambda/(lambda+n_aug);
double weight = 0.5/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) {
weights(i) = weight;
}
// create example matrix with predicted sigma points in state space
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create example vector for predicted state mean
VectorXd x = VectorXd(n_x);
x <<
5.93637,
1.49035,
2.20528,
0.536853,
0.353577;
// create example matrix for predicted state covariance
MatrixXd P = MatrixXd(n_x,n_x);
P <<
0.0054342, -0.002405, 0.0034157, -0.0034819, -0.00299378,
-0.002405, 0.01084, 0.001492, 0.0098018, 0.00791091,
0.0034157, 0.001492, 0.0058012, 0.00077863, 0.000792973,
-0.0034819, 0.0098018, 0.00077863, 0.011923, 0.0112491,
-0.0029937, 0.0079109, 0.00079297, 0.011249, 0.0126972;
// create example matrix with sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug + 1);
Zsig <<
6.1190, 6.2334, 6.1531, 6.1283, 6.1143, 6.1190, 6.1221, 6.1190, 6.0079, 6.0883, 6.1125, 6.1248, 6.1190, 6.1188, 6.12057,
0.24428, 0.2337, 0.27316, 0.24616, 0.24846, 0.24428, 0.24530, 0.24428, 0.25700, 0.21692, 0.24433, 0.24193, 0.24428, 0.24515, 0.245239,
2.1104, 2.2188, 2.0639, 2.187, 2.0341, 2.1061, 2.1450, 2.1092, 2.0016, 2.129, 2.0346, 2.1651, 2.1145, 2.0786, 2.11295;
// create example vector for mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
z_pred <<
6.12155,
0.245993,
2.10313;
// create example matrix for predicted measurement covariance
MatrixXd S = MatrixXd(n_z,n_z);
S <<
0.0946171, -0.000139448, 0.00407016,
-0.000139448, 0.000617548, -0.000770652,
0.00407016, -0.000770652, 0.0180917;
// create example vector for incoming radar measurement
VectorXd z = VectorXd(n_z);
z <<
5.9214, // rho in m
0.2187, // phi in rad
2.0062; // rho_dot in m/s
// create matrix for cross correlation Tc
MatrixXd Tc = MatrixXd(n_x, n_z);
/**
* Student part begin
*/
// calculate cross correlation matrix
Tc.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; ++i) { // 2n+1 simga points
// residual
VectorXd z_diff = Zsig.col(i) - z_pred;
// angle normalization
while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
// state difference
VectorXd x_diff = Xsig_pred.col(i) - x;
// angle normalization
while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
Tc = Tc + weights(i) * x_diff * z_diff.transpose();
}
// Kalman gain K;
MatrixXd K = Tc * S.inverse();
// residual
VectorXd z_diff = z - z_pred;
// angle normalization
while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
// update state mean and covariance matrix
x = x + K * z_diff;
P = P - K*S*K.transpose();
/**
* Student part end
*/
// print result
std::cout << "Updated state x: " << std::endl << x << std::endl;
std::cout << "Updated state covariance P: " << std::endl << P << std::endl;
// write result
*x_out = x;
*P_out = P;
}
/**
* expected result x:
* x =
* 5.92276
* 1.41823
* 2.15593
* 0.489274
* 0.321338
*/
/**
* expected result P:
* P =
* 0.00361579 -0.000357881 0.00208316 -0.000937196 -0.00071727
* -0.000357881 0.00539867 0.00156846 0.00455342 0.00358885
* 0.00208316 0.00156846 0.00410651 0.00160333 0.00171811
* -0.000937196 0.00455342 0.00160333 0.00652634 0.00669436
* -0.00071719 0.00358884 0.00171811 0.00669426 0.00881797
31 Parameters and Consistency
In this lesson ,I would like to discuss with you how to choose noise parameters and how to evaluate this choice . This is not only related to the unscented Kalmen filter , but it can be applied to any Bayesian filter .
Let's staet with the measurement noise . This noise describe how precise the sensor can measure , it means we have to look into the sensor manual and see how the precise the sensor is .
For example , we might read the radar can measure the distance rho with the standard deviation of 0.3 meters . This means we know the value for the variance sigma rho square .
It's difficult to choose the process noise ,in reality ,objects in a traffic environment don't move with wide acceleration noise . (在现实中，交通环境的对象移动时的加速噪声不是白噪声)This means a driver constantly switch between gas and brake . So we really apply a strong approximation here . assuming a wide process noise .
If this is a good value for you really depends on your application , Is it important for your application to react fast on changes ? Then you choose the process noise a little higher .
Is it important to provide smooth estimations ? Then you choose the process noise a little lower .
Usually you want to know if you set up the noise parameters correctly . For that purpose ,you can run a consistency check on the filter .
Of course , these are all stochastic processes . And it might just be a natural coincidence . But if the result keep looking like this , this explanation is unlike . This result tells you that you underestimate the uncertainty . In your system , your e stimate is less precise than you think .
A filter is consistent if it provides a realistic estimation uncertainy . It is very easy to check the consistency of your filter , and I recommend to always do that when you design a filter . An important consistency check is called NIS, it means Normalized Innovation Squared . (归一化新息平方)
The innovation is the difference between the predicted measurement and the actual measuremnet .And normalized means you put relation to the covariance matrix S.That is why you have the inverse of the matrix S .
The NIS value follow a distribution which is called chi-squared distribution . (卡方分布). If you google chi-square distribution , you will find a table like this , And this table tells you the number you should expect for your NIS .
df means degress of freedom , that's the dimension of our measurement space . We have a three-dimensional radar measurement . So we have three degree of freedom ,so what do all these
Unfortunately , the .NIS test doesn't tell you where the mistake comes from , but it gives you at least some feedback .
If you get something like this ,then everything is good .
Process Noise and the UKF Project
For the CTRV model, two parameters define the process noise:
- representing longitudinal acceleration noise (you might see this referred to as linear acceleration)
- representing yaw acceleration noise (this is also called angular acceleration)
In the project, both of these values will need to be tuned. You will have to test different values in order to get a working solution. In the video, Dominik mentions using *= 9 s a starting point when tracking a vehicle. In the UKF project, you will be tracking a bicycle rather than a vehicle. So 9 might not be an appropriate acceleration noise parameter. Tuning will involve:
- guessing appropriate parameter values
- running the UKF filter
- deciding if the results are good enough
- tweaking the parameters and repeating the process
线速度就等于 v = w * s
Measurement Noise Parameters
Measurement noise parameters represent uncertainty in sensor measurements. In general, the manufacturer will provide these values in the sensor manual. In the UKF project, you will not need to tune these parameters.
就是将你得预测的观测信号与实际的观测信号做比较，看是否满足NIC原则？
32 What to Expect from the Project
I would like to point out some things about UKF I find quite remarkable . We see here a bicycle that is first driving straight and then turning into a circle .
The green line is the sequence of all measurements we receive both from LiDAR and Radar . As before ,these measurements are quite nosiy .
The orange dot are the estimation resource of the UKF fusing laser and radar .
Remember , the linear process model in the last project had problems following the turn .(在跟踪转弯的时候碰到了问题).
The CTRV model we're using this time follows the turn quite nicely , and still provides a smooth position estimate .
You can play around with the process noise values , and make the estimation even smoother .Or force it to follow the measurements quicker . When you change the process noise values , also make sure to check the consistency of your filter .
Remember if the UKF is consistent , it means it provides a realistic covariance metrics .
The UKF also estimates the velocity of the bicycle of couese . It can do it with or without radar . You will find that the velocity estimate converges much faster if you use radar too .
What I find really impressive is how precise the UKF can estimate the orientation of the bicycle . None of our sensors is able to directly observe the orientation , but we still get a precise estimate .
Even the yaw rate can be estimated providing useful results .
Let's summarize the three most important probabilities of UKF as a sensr fusion tool for self driving car.
You will be able to take noisy measurement data as input and provide a smooth position and velocity estimation of dynamic objects around you ,without introducing a delay .
Number two , you can provide an estimation of the orientation and the yaw rate of other vehicles using sensor that can't even directly observe these thing .
The UKF also give information on how precise the result is .Because it always provide a covariance matrix for every estimation . And you know that this covariance matrix is realistic if the UKF performs a consistency check .
The uncertainy of the estimation result is very important for self-driving cars .Because if the position of your leading vehicle is quite uncertain at some time , you better keep a litter more distance .
接下来的一个月可能这个专栏都不会更新了，要去认真学一下一些理论知识了，到时候再来分享了，
345
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
