simplify and better
This commit is contained in:
parent
bbb37309fc
commit
c32ebcb2c6
|
@ -20,9 +20,9 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
|
|||
}
|
||||
|
||||
A <<
|
||||
1, dt, 0, 0,
|
||||
1, 0, 0, 0,
|
||||
0, 1, 0, 0,
|
||||
0, 0, 1, dt,
|
||||
0, 0, 1, 0,
|
||||
0, 0, 0, 1;
|
||||
|
||||
x = A * x;
|
||||
|
@ -46,7 +46,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
|
|||
}
|
||||
|
||||
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
|
||||
double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
|
||||
double R = pow(10 * camera_odometry.getRotStd()[2], 2);
|
||||
double meas = camera_odometry.getRot()[2];
|
||||
update_state(C_posenet, R, current_time, meas);
|
||||
|
||||
|
@ -71,21 +71,21 @@ Localizer::Localizer() {
|
|||
0, 0, 0, 1;
|
||||
|
||||
Q <<
|
||||
pow(.1, 2.0), 0, 0, 0,
|
||||
0, 0, 0, 0,
|
||||
0, pow(0.1, 2.0), 0, 0,
|
||||
0, 0, 0, 0,
|
||||
0, 0, pow(0.005 / 100.0, 2.0), 0;
|
||||
0, 0, pow(0.005/ 100.0, 2.0), 0,
|
||||
0, 0, 0, 0;
|
||||
P <<
|
||||
pow(100.0, 2.0), 0, 0, 0,
|
||||
0, pow(100.0, 2.0), 0, 0,
|
||||
0, 0, pow(100.0, 2.0), 0,
|
||||
0, 0, 0, pow(100.0, 2.0);
|
||||
pow(10000.0, 2.0), 0, 0, 0,
|
||||
0, pow(10000.0, 2.0), 0, 0,
|
||||
0, 0, pow(10000.0, 2.0), 0,
|
||||
0, 0, 0, pow(10000.0, 2.0);
|
||||
|
||||
C_posenet << 1, 0, 0, 0;
|
||||
C_gyro << 1, 0, 1, 0;
|
||||
x << 0, 0, 0, 0;
|
||||
|
||||
R_gyro = pow(0.25, 2.0);
|
||||
R_gyro = pow(0.025, 2.0);
|
||||
}
|
||||
|
||||
void Localizer::handle_log(cereal::Event::Reader event) {
|
||||
|
|
Loading…
Reference in a new issue