void TestLeastSquares() {
MatrixXf A = MatrixXf::Random(10, 2);
VectorXf b = VectorXf::Random(10);
Vector2f x;
std::cout << "=============================" << std::endl;
std::cout << "Testing least squares solvers" << std::endl;
std::cout << "=============================" << std::endl;
x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
std::cout << "Solution using Jacobi SVD = " << x.transpose() << std::endl;
x = A.colPivHouseholderQr().solve(b);
std::cout << "Solution using column pivoting Householder QR = " << x.transpose() << std::endl;
// If the matrix A is ill-conditioned, then this is not a good method
x = (A.transpose() * A).ldlt().solve(A.transpose() * b);
std::cout << "Solution using normal equation = " << x.transpose() << std::endl;
}
Matrix2f CameraModel::diff_distort_undistort(Vector2f hn) {
float r_2 = hn(0)*hn(0) + hn(1)*hn(1);
float l = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;
Vector2f hn_contr;
hn_contr << hn(1),hn(0);
Vector2f pv;
pv << p1,p2;
Vector2f pv_contr;
pv_contr << p2,p1;
Matrix2f j_matrix;
j_matrix << p2*hn(0), 0, 0, p1*hn(1);
float f = k1 + 2*k2*r_2 + 3*k3*r_2*r_2;
Matrix2f Jacobian_Distort_Undistort = l*Matrix2f::Identity() + 2*f*hn*hn.transpose() + 2*pv*hn_contr.transpose() + 2*pv_contr*hn.transpose() + 4*j_matrix;
return Jacobian_Distort_Undistort;
}
void BlockLocalPositionEstimator::correctFlow()
{
// flow measurement matrix and noise matrix
Matrix<float, n_y_flow, n_x> C;
C.setZero();
C(Y_flow_x, X_x) = 1;
C(Y_flow_y, X_y) = 1;
Matrix<float, n_y_flow, n_y_flow> R;
R.setZero();
R(Y_flow_x, Y_flow_x) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
R(Y_flow_y, Y_flow_y) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
float flow_speed[3] = {0.0f, 0.0f, 0.0f};
float global_speed[3] = {0.0f, 0.0f, 0.0f};
/* calc dt between flow timestamps */
/* ignore first flow msg */
if (_time_last_flow == 0) {
_time_last_flow = _sub_flow.get().timestamp;
return;
}
float dt = (_sub_flow.get().timestamp - _time_last_flow) * 1.0e-6f ;
_time_last_flow = _sub_flow.get().timestamp;
// calculate velocity over ground
if (_sub_flow.get().integration_timespan > 0) {
flow_speed[0] = (_sub_flow.get().pixel_flow_x_integral /
(_sub_flow.get().integration_timespan / 1e6f) -
_sub_att.get().pitchspeed) * // Body rotation correction TODO check this
_x(X_z);
flow_speed[1] = (_sub_flow.get().pixel_flow_y_integral /
(_sub_flow.get().integration_timespan / 1e6f) -
_sub_att.get().rollspeed) * // Body rotation correction
_x(X_z);
} else {
flow_speed[0] = 0;
flow_speed[1] = 0;
}
flow_speed[2] = 0.0f;
/* update filtered flow */
_pub_filtered_flow.get().sumx += flow_speed[0] * dt;
_pub_filtered_flow.get().sumy += flow_speed[1] * dt;
_pub_filtered_flow.get().vx = flow_speed[0];
_pub_filtered_flow.get().vy = flow_speed[1];
// TODO add yaw rotation correction (with distance to vehicle zero)
// convert to globalframe velocity
for (uint8_t i = 0; i < 3; i++) {
float sum = 0.0f;
for (uint8_t j = 0; j < 3; j++) {
sum += flow_speed[j] * PX4_R(_sub_att.get().R, i, j);
}
global_speed[i] = sum;
}
// flow integral
_flowX += global_speed[0] * dt;
_flowY += global_speed[1] * dt;
// measurement
Vector2f y;
y(0) = _flowX;
y(1) = _flowY;
// residual
Vector2f r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
(C * _P * C.transpose() + R).inverse();
// fault detection
float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
if (_sub_flow.get().quality < MIN_FLOW_QUALITY) {
if (!_flowFault) {
mavlink_log_info(_mavlink_fd, "[lpe] bad flow data ");
warnx("[lpe] bad flow data ");
_flowFault = FAULT_SEVERE;
}
} else if (beta > _beta_max.get()) {
if (!_flowFault) {
mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta));
warnx("[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] flow OK");
warnx("[lpe] flow OK");
}
// kalman filter correction if no fault
if (_flowFault == FAULT_NONE) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
_x += K * r;
_P -= K * C * _P;
// reset flow integral to current estimate of position
// if a fault occurred
} else {
_flowX = _x(X_x);
_flowY = _x(X_y);
}
}
Vector4f pointAlignerIteration(Vector3f x, MatrixXf Z){
Matrix3f H;
Vector3f b;
Vector4f xNew_chi;
H << 0,0,0,
0,0,0,
0,0,0;
b << 0,
0,
0;
float chi = 0;
Matrix3f X = v2tRad(x);
//cout << "matrix X from inside the point align iteration: \n" << X << endl;
Matrix2f Omega;
Omega << 1, 0,
0, 1;
Vector2f e;
Jacobian jac;
for(int i=0; i < Z.cols(); i++){
e = computeError(i, X, Z);
jac = computeJacobian(i, X, Z);
//cout << "jacobian: \n" << jac << endl;
//getc(stdin);
H += jac.transpose()*Omega*jac;
b += jac.transpose()*Omega*e;
chi += e.transpose()*Omega*e;
}
Vector3f dX;
dX = -H.inverse()*b;
//cout << "H inverse: \n" << H.inverse() << endl;
//getc(stdin);
for(int i=0; i<3; i++){
xNew_chi(i, 0) = x(i, 0) + dX(i, 0);
}
xNew_chi(2, 0) = atan2(sin(xNew_chi(2,0)), cos(xNew_chi(2, 0)));
xNew_chi(3, 0) = chi;
return xNew_chi;
}
