mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-30 19:33:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Locationd 100 Hz (#20816)

Browse Source 
* fix std transform

* 100Hz

* new ref

* no more decimation

* clean up confusing maths

* static typing

* Revert "static typing"

This reverts commit 23d87337de648e629fbd35dd8c04a740bbefca47.

* 100Hz costs more

* move normalization into core

* add quat idxs

* add big eps

* this is not safe in the filter

* more sensible

* updates to rednose

* not tested

* normalize in python too

* update rednose

* nan check

* check for infs too

* all should be finite

* update ref

* rednose pr now in master

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
old-commit-hash: e9db5723ef348954118643501a92cf0715402fea
This commit is contained in:
Willem Melching
2021-05-06 11:01:58 +02:00
committed by GitHub
parent 01f1d04f98
commit b4263a43fc
9 changed files with 61 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/live_kf.cc
+3 -3
View File
@@ -39,8 +39,9 @@ LiveKalman::LiveKalman() {
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x), get_mapmat(initial_P),
this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(), std::vector<std::string>(), 0.2);
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.2);
}
void LiveKalman::init_state(VectorXd& state, VectorXd& covs_diag, double filter_time) {
@@ -92,7 +93,6 @@ std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std:
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
break;
}
this->filter->normalize_state(STATE_ECEF_ORIENTATION_START, STATE_ECEF_ORIENTATION_END);
return r;
}
selfdrive/locationd/models/live_kf.py
+2 -2
View File
@@ -55,8 +55,8 @@ class LiveKalman():
# state covariance
initial_P_diag = np.array([1e16, 1e16, 1e16,
1e6, 1e6, 1e6,
1e4, 1e4, 1e4,
10**2, 10**2, 10**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
0.05**2, 0.05**2, 0.05**2,
0.02**2,
selfdrive/locationd/models/loc_kf.py
+3 -6
View File
@@ -382,9 +382,11 @@ class LocKalman():
self.computer = LstSqComputer(generated_dir, N)
self.max_tracks = max_tracks
self.quaternion_idxs = [3,] + [(self.dim_main + i*self.dim_augment + 3)for i in range(self.N)]
# init filter
self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds)
N, self.dim_augment, self.dim_augment_err, self.maha_test_kinds, self.quaternion_idxs)
@property
def x(self):
@@ -453,11 +455,6 @@ class LocKalman():
# Should not continue if the quats behave this weirdly
if not 0.1 < quat_norm < 10:
raise RuntimeError("Sir! The filter's gone all wobbly!")
self.filter.normalize_state(3, 7)
for i in range(self.N):
d1 = self.dim_main
d3 = self.dim_augment
self.filter.normalize_state(d1 + d3 * i + 3, d1 + d3 * i + 7)
return r
def get_R(self, kind, n):