mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-21 17:22:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove unused states from locationd yawrate kalman filer

Browse Source 
old-commit-hash: a53577ed35
This commit is contained in:
Willem Melching
2020-02-03 12:23:41 -08:00
parent 7022910555
commit 8c318e807f
2 changed files with 25 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd_yawrate.cc
+17 -25
View File
@@ -10,7 +10,7 @@
#include "locationd_yawrate.h"
void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas) {
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
double dt = current_time - prev_update_time;
if (dt < 0) {
@@ -19,18 +19,12 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
prev_update_time = current_time;
}
A <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
x = A * x;
P = A * P * A.transpose() + dt * Q;
double y = meas - C * x;
double S = R + C * P * C.transpose();
Eigen::Vector4d K = P * C.transpose() * (1.0 / S);
Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
x = x + K * y;
P = (I - K * C) * P;
}
@@ -63,27 +57,25 @@ void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_sta
Localizer::Localizer() {
// States: [yaw rate, yaw rate diff, gyro bias, gyro bias diff]
I <<
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
// States: [yaw rate, gyro bias]
A <<
1, 0,
0, 1;
Q <<
pow(.1, 2.0), 0, 0, 0,
0, 0, 0, 0,
0, 0, pow(0.005/ 100.0, 2.0), 0,
0, 0, 0, 0;
pow(.1, 2.0), 0,
0, pow(0.005/ 100.0, 2.0),
P <<
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);
pow(10000.0, 2.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;
I <<
1, 0,
0, 1;
C_posenet << 1, 0;
C_gyro << 1, 1;
x << 0, 0;
R_gyro = pow(0.025, 2.0);
}
selfdrive/locationd/locationd_yawrate.h
+8 -8
View File
@@ -7,22 +7,22 @@
class Localizer
{
Eigen::Matrix4d A;
Eigen::Matrix4d I;
Eigen::Matrix4d Q;
Eigen::Matrix<double, 1, 4> C_posenet;
Eigen::Matrix<double, 1, 4> C_gyro;
Eigen::Matrix2d A;
Eigen::Matrix2d I;
Eigen::Matrix2d Q;
Eigen::Matrix<double, 1, 2> C_posenet;
Eigen::Matrix<double, 1, 2> C_gyro;
double R_gyro;
void update_state(const Eigen::Matrix<double, 1, 4> &C, const double R, double current_time, double meas);
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
public:
Eigen::Vector4d x;
Eigen::Matrix4d P;
Eigen::Vector2d x;
Eigen::Matrix2d P;
double steering_angle = 0;
double car_speed = 0;
double posenet_speed = 0;