Compare commits

..

1 Commits

Author SHA1 Message Date
Jason Wen c78f4c4549 agnos: 19.9 Bluetooth test build 2026-11-21 16:15:12 -05:00
35 changed files with 2046 additions and 308 deletions
@@ -168,7 +168,7 @@ jobs:
echo "Building locationd..."
# -j1: parallel rednose generators OOM the device
scons -j1 cache_dir="$SCONS_CACHE" --minimal \
openpilot/selfdrive/locationd
openpilot/selfdrive/locationd openpilot/sunnypilot/selfdrive/locationd
echo "Building rest of sunnypilot"
sudo rm -rf /tmp/* || true
mkdir -p "$BUILD_DIR/tmp"
+2
View File
@@ -293,6 +293,8 @@ SConscript([
'openpilot/selfdrive/ui/SConscript',
])
SConscript(['openpilot/sunnypilot/SConscript'])
# Build desktop-only tools
if GetOption('extras') and arch != "comma_arm64":
SConscript([
+1 -1
View File
@@ -164,7 +164,7 @@ A supported vehicle is one that just works when you install a comma device. All
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Diesel 2019">Buy Here</a></sub></details>|||
|Hyundai|Tucson Hybrid 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid 2022-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Hybrid (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|Hyundai|Tucson Plug-in Hybrid 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2023-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2024">Buy Here</a></sub></details>|||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Veloster 2019-20">Buy Here</a></sub></details>|||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2019-21">Buy Here</a></sub></details>|||
+1 -1
View File
@@ -16,7 +16,7 @@ export VECLIB_MAXIMUM_THREADS=1
export QCOM_PRIORITY=12
if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="19.9"
export AGNOS_VERSION="19.9-3fe820c"
fi
export STAGING_ROOT="/data/safe_staging"
+1 -10
View File
@@ -483,16 +483,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
}
}
struct GpsLocationSP @0xcb9fd56c7057593a {
latitude @0 :Float64;
longitude @1 :Float64;
speed @2 :Float32;
bearingDeg @3 :Float32;
horizontalAccuracy @4 :Float32; # m
bearingAccuracyDeg @5 :Float32;
hasFix @6 :Bool;
gpsOK @7 :Bool; # a raw fix arrived in the last 2 s
fused @8 :Bool; # false while passing the raw fix through
struct CustomReserved10 @0xcb9fd56c7057593a {
}
struct CustomReserved11 @0xc2243c65e0340384 {
+2 -2
View File
@@ -2648,7 +2648,7 @@ struct Event {
carStateSP @114 :Custom.CarStateSP;
liveMapDataSP @115 :Custom.LiveMapDataSP;
modelDataV2SP @116 :Custom.ModelDataV2SP;
gpsLocationSP @136 :Custom.GpsLocationSP;
customReserved10 @136 :Custom.CustomReserved10;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
@@ -2701,7 +2701,7 @@ struct Event {
lateralPlanDEPRECATED @64 :LateralPlan;
navModelDEPRECATED @104 :Deprecated.NavModelData;
uiPlanDEPRECATED @106 :UiPlan;
liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
liveLocationKalman @72 :LiveLocationKalman;
liveTracksDEPRECATED @16 :List(Deprecated.LiveTracksDEPRECATED);
onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED);
gyroscope2DEPRECATED @100 :SensorEventData;
+1 -1
View File
@@ -95,7 +95,7 @@ _services: dict[str, tuple] = {
"carStateSP": (True, 100., 10),
"liveMapDataSP": (True, 1., 1),
"modelDataV2SP": (True, 20., 10, QueueSize.BIG),
"gpsLocationSP": (True, 20., 20),
"liveLocationKalman": (True, 20.),
# debug
"uiDebug": (True, 0., 1),
+16 -16
View File
@@ -1,7 +1,7 @@
[
{
"name": "xbl",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-e8acf2a9cc7f0ce84cb803bfea9477f765c0d7b4daf26048e59651b9e6a7bfbb.img.xz",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/xbl-e8acf2a9cc7f0ce84cb803bfea9477f765c0d7b4daf26048e59651b9e6a7bfbb.img.xz",
"hash": "e8acf2a9cc7f0ce84cb803bfea9477f765c0d7b4daf26048e59651b9e6a7bfbb",
"hash_raw": "e8acf2a9cc7f0ce84cb803bfea9477f765c0d7b4daf26048e59651b9e6a7bfbb",
"size": 3282256,
@@ -12,7 +12,7 @@
},
{
"name": "xbl_config",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-758552ecf92b5569677197783bf0ccb73d7f961685308e45d3276ac9dd974f85.img.xz",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/xbl_config-758552ecf92b5569677197783bf0ccb73d7f961685308e45d3276ac9dd974f85.img.xz",
"hash": "758552ecf92b5569677197783bf0ccb73d7f961685308e45d3276ac9dd974f85",
"hash_raw": "758552ecf92b5569677197783bf0ccb73d7f961685308e45d3276ac9dd974f85",
"size": 98124,
@@ -23,7 +23,7 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-29fd7ed1c012e599420764840f9f11286d34dbff4adaf102a447f06d8c5e0b35.img.xz",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/abl-29fd7ed1c012e599420764840f9f11286d34dbff4adaf102a447f06d8c5e0b35.img.xz",
"hash": "29fd7ed1c012e599420764840f9f11286d34dbff4adaf102a447f06d8c5e0b35",
"hash_raw": "29fd7ed1c012e599420764840f9f11286d34dbff4adaf102a447f06d8c5e0b35",
"size": 274432,
@@ -34,7 +34,7 @@
},
{
"name": "aop",
"url": "https://commadist.azureedge.net/agnosupdate/aop-78b2287ca219a0811b3004c523fa0f4749e4d1fd92be3aba61699305b7943ad1.img.xz",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/aop-78b2287ca219a0811b3004c523fa0f4749e4d1fd92be3aba61699305b7943ad1.img.xz",
"hash": "78b2287ca219a0811b3004c523fa0f4749e4d1fd92be3aba61699305b7943ad1",
"hash_raw": "78b2287ca219a0811b3004c523fa0f4749e4d1fd92be3aba61699305b7943ad1",
"size": 184364,
@@ -45,7 +45,7 @@
},
{
"name": "devcfg",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-f71df3a86958c093ba3969254c4db025187eef9385427f1ade946742939b43cc.img.xz",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/devcfg-f71df3a86958c093ba3969254c4db025187eef9385427f1ade946742939b43cc.img.xz",
"hash": "f71df3a86958c093ba3969254c4db025187eef9385427f1ade946742939b43cc",
"hash_raw": "f71df3a86958c093ba3969254c4db025187eef9385427f1ade946742939b43cc",
"size": 40336,
@@ -56,28 +56,28 @@
},
{
"name": "boot",
"url": "https://commadist.azureedge.net/agnosupdate/boot-b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615.img.xz",
"hash": "b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615",
"hash_raw": "b9c9b9261fca0cef9e6d9b198fa4b563767e6342afc7a61ab137b2b519260615",
"size": 46962688,
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/boot-7c3896fa541f0ad8de8ef9998009dee036cc763cd200aa0237c7b9cf02dfc55e.img.xz",
"hash": "7c3896fa541f0ad8de8ef9998009dee036cc763cd200aa0237c7b9cf02dfc55e",
"hash_raw": "7c3896fa541f0ad8de8ef9998009dee036cc763cd200aa0237c7b9cf02dfc55e",
"size": 47859712,
"sparse": false,
"full_check": true,
"has_ab": true,
"ondevice_hash": "3ef4769bb1cc73bfc31f96f073b46af42dcc0bad4ab2dde32d11929745d0cd28"
"ondevice_hash": "d6c7d933e6b20fa8eb2bf5d349c6438d93f77eb5b195c117f2b7b7f986960ecb"
},
{
"name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7.img.xz",
"hash": "9aebca09b9d142718e71efbfe44b723010eec6e70067c043c63186067291739d",
"hash_raw": "80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/system-208d49da0e495ea529f69186b1e06e3eb2b80898435b4595bf2379de6f20fa6c.img.xz",
"hash": "220162cfc92642a1704588c13ba65c98febce48d80ca0f5267133b860c776524",
"hash_raw": "208d49da0e495ea529f69186b1e06e3eb2b80898435b4595bf2379de6f20fa6c",
"size": 4718592000,
"sparse": true,
"full_check": false,
"has_ab": true,
"ondevice_hash": "270b76c5c5568828e3883390b83ea445425a104dd42119d68f06093ce6b6fcef",
"ondevice_hash": "e52fd5539011d7ed3a6fc9915be003ebbe0b15391b09043285ca2c9ef5f6f193",
"alt": {
"hash": "80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7",
"url": "https://commadist.azureedge.net/agnosupdate/system-80d9373348ddff2a5d1f534fab3420383939c96f3eba60baab6b15339fc602c7.img",
"hash": "208d49da0e495ea529f69186b1e06e3eb2b80898435b4595bf2379de6f20fa6c",
"url": "https://huggingface.co/sunnypilot/agnos-ci/resolve/main/pr-6/system-208d49da0e495ea529f69186b1e06e3eb2b80898435b4595bf2379de6f20fa6c.img",
"size": 4718592000
}
}
+1 -1
View File
@@ -178,7 +178,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
{"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"LastGPSPositionFused", {PERSISTENT, STRING}},
{"LastGPSPositionLLK", {PERSISTENT, STRING}},
{"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
{"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
+2
View File
@@ -0,0 +1,2 @@
SConscript(['common/transformations/SConscript'])
SConscript(['selfdrive/locationd/SConscript'])
@@ -0,0 +1,4 @@
Import('env')
transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
Export('transformations')
@@ -0,0 +1,100 @@
#define _USE_MATH_DEFINES
#include "sunnypilot/common/transformations/coordinates.hpp"
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>
double a = 6378137; // lgtm [cpp/short-global-name]
double b = 6356752.3142; // lgtm [cpp/short-global-name]
double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
double e1sq = 6.73949674228 * 0.001;
static Geodetic to_degrees(Geodetic geodetic){
geodetic.lat = RAD2DEG(geodetic.lat);
geodetic.lon = RAD2DEG(geodetic.lon);
return geodetic;
}
static Geodetic to_radians(Geodetic geodetic){
geodetic.lat = DEG2RAD(geodetic.lat);
geodetic.lon = DEG2RAD(geodetic.lon);
return geodetic;
}
ECEF geodetic2ecef(const Geodetic &geodetic) {
auto g = to_radians(geodetic);
double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2));
double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon);
double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon);
double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat);
return {x, y, z};
}
Geodetic ecef2geodetic(const ECEF &e) {
// Convert from ECEF to geodetic using Ferrari's methods
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
double x = e.x;
double y = e.y;
double z = e.z;
double r = sqrt(x * x + y * y);
double Esq = a * a - b * b;
double F = 54 * b * b * z * z;
double G = r * r + (1 - esq) * z * z - esq * Esq;
double C = (esq * esq * F * r * r) / (pow(G, 3));
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
double Q = sqrt(1 + 2 * esq * esq * P);
double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
double U = sqrt(pow((r - esq * r_0), 2) + z * z);
double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z);
double Z_0 = b * b * z / (a * V);
double h = U * (1 - b * b / (a * V));
double lat = atan((z + e1sq * Z_0) / r);
double lon = atan2(y, x);
return to_degrees({lat, lon, h});
}
LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) {
init_ecef << e.x, e.y, e.z;
auto g = to_radians(geodetic);
ned2ecef_matrix <<
-sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
-sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon),
cos(g.lat), 0, -sin(g.lat);
ecef2ned_matrix = ned2ecef_matrix.transpose();
}
NED LocalCoord::ecef2ned(const ECEF &e) {
Eigen::Vector3d ecef;
ecef << e.x, e.y, e.z;
Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef));
return {ned[0], ned[1], ned[2]};
}
ECEF LocalCoord::ned2ecef(const NED &n) {
Eigen::Vector3d ned;
ned << n.n, n.e, n.d;
Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef;
return {ecef[0], ecef[1], ecef[2]};
}
NED LocalCoord::geodetic2ned(const Geodetic &g) {
ECEF e = ::geodetic2ecef(g);
return ecef2ned(e);
}
Geodetic LocalCoord::ned2geodetic(const NED &n) {
ECEF e = ned2ecef(n);
return ::ecef2geodetic(e);
}
@@ -0,0 +1,43 @@
#pragma once
#include <eigen3/Eigen/Dense>
#define DEG2RAD(x) ((x) * M_PI / 180.0)
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
struct ECEF {
double x, y, z;
Eigen::Vector3d to_vector() const {
return Eigen::Vector3d(x, y, z);
}
};
struct NED {
double n, e, d;
Eigen::Vector3d to_vector() const {
return Eigen::Vector3d(n, e, d);
}
};
struct Geodetic {
double lat, lon, alt;
bool radians=false;
};
ECEF geodetic2ecef(const Geodetic &g);
Geodetic ecef2geodetic(const ECEF &e);
class LocalCoord {
public:
Eigen::Matrix3d ned2ecef_matrix;
Eigen::Matrix3d ecef2ned_matrix;
Eigen::Vector3d init_ecef;
LocalCoord(const Geodetic &g, const ECEF &e);
LocalCoord(const Geodetic &g) : LocalCoord(g, ::geodetic2ecef(g)) {}
LocalCoord(const ECEF &e) : LocalCoord(::ecef2geodetic(e), e) {}
NED ecef2ned(const ECEF &e);
ECEF ned2ecef(const NED &n);
NED geodetic2ned(const Geodetic &g);
Geodetic ned2geodetic(const NED &n);
};
@@ -0,0 +1,143 @@
#define _USE_MATH_DEFINES
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>
#include "sunnypilot/common/transformations/orientation.hpp"
#include "sunnypilot/common/transformations/coordinates.hpp"
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat) {
if (quat.w() > 0){
return quat;
} else {
return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z());
}
}
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) {
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
return ensure_unique(q);
}
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat) {
// TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
// return {euler(2), euler(1), euler(0)};
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
double theta = asin(asin_arg_clipped);
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
return {gamma, theta, psi};
}
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) {
return quat.toRotationMatrix();
}
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) {
return ensure_unique(Eigen::Quaterniond(rot));
}
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) {
return quat2rot(euler2quat(euler));
}
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) {
return quat2euler(rot2quat(rot));
}
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) {
return euler2rot({roll, pitch, yaw});
}
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle) {
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(angle, axis);
return q.toRotationMatrix();
}
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) {
/*
Using Rotations to Build Aerospace Coordinate Systems
Don Koks
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
*/
LocalCoord converter = LocalCoord(ecef_init);
Eigen::Vector3d zero = ecef_init.to_vector();
Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0;
Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0;
Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0;
Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1;
Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1;
Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1;
Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2;
Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2;
x0 = Eigen::Vector3d(1, 0, 0);
y0 = Eigen::Vector3d(0, 1, 0);
z0 = Eigen::Vector3d(0, 0, 1);
double psi = atan2(x3.dot(y0), x3.dot(x0));
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
y2 = rot(z0, psi) * y0;
z2 = rot(y2, theta) * z0;
double phi = atan2(y3.dot(z2), y3.dot(y2));
return {phi, theta, psi};
}
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) {
/*
Using Rotations to Build Aerospace Coordinate Systems
Don Koks
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
*/
LocalCoord converter = LocalCoord(ecef_init);
Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0);
Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0);
Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1);
Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0;
Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0;
Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0;
Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1;
Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1;
Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1;
Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2;
Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2;
Eigen::Vector3d zero = ecef_init.to_vector();
x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
double psi = atan2(x3.dot(y0), x3.dot(x0));
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
y2 = rot(z0, psi) * y0;
z2 = rot(y2, theta) * z0;
double phi = atan2(y3.dot(z2), y3.dot(y2));
return {phi, theta, psi};
}
@@ -0,0 +1,17 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include "sunnypilot/common/transformations/coordinates.hpp"
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat);
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler);
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat);
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat);
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot);
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler);
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot);
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw);
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle);
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose);
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose);
@@ -19,12 +19,12 @@ class BaseMapData(ABC):
def __init__(self):
self.params = Params()
self.sm = messaging.SubMaster(['gpsLocationSP'])
self.sm = messaging.SubMaster(['liveLocationKalman'])
self.pm = messaging.PubMaster(['liveMapDataSP'])
self.localizer_valid = False
self.last_bearing = None
self.last_position = coordinate_from_param("LastGPSPositionFused", self.params)
self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
@abstractmethod
def update_location(self) -> None:
@@ -47,7 +47,7 @@ class BaseMapData(ABC):
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
mapd_sp_send = messaging.new_message('liveMapDataSP')
mapd_sp_send.valid = self.sm['gpsLocationSP'].gpsOK
mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
live_map_data = mapd_sp_send.liveMapDataSP
live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
@@ -5,8 +5,10 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
from openpilot.sunnypilot.navd.helpers import Coordinate
@@ -18,12 +20,12 @@ class OsmMapData(BaseMapData):
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
def update_location(self) -> None:
location = self.sm['gpsLocationSP']
self.localizer_valid = self.sm.valid['gpsLocationSP'] and location.hasFix
location = self.sm['liveLocationKalman']
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
if self.localizer_valid:
self.last_bearing = location.bearingDeg
self.last_position = Coordinate(location.latitude, location.longitude)
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
if self.last_position is None:
return
@@ -5,9 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
@@ -18,26 +20,28 @@ class TestOsmMapData(OpenpilotTestCase):
LATITUDE = 32.875134
LONGITUDE = -117.209777
ALTITUDE = 42.0
BEARING = 90.0
def setup_method(self):
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else Params()
self.mem_params.remove("LastGPSPosition")
self.map_data = OsmMapData()
self.pm = messaging.PubMaster(['gpsLocationSP'])
self.pm = messaging.PubMaster(['liveLocationKalman'])
def location_message(self, has_fix: bool):
msg = messaging.new_message('gpsLocationSP', valid=has_fix)
location = msg.gpsLocationSP
location.hasFix = has_fix
location.latitude, location.longitude, location.bearingDeg = self.LATITUDE, self.LONGITUDE, self.BEARING
def location_message(self, status: log.LiveLocationKalman.Status, position_valid: bool):
msg = messaging.new_message('liveLocationKalman', valid=True)
location = msg.liveLocationKalman
location.status = status
location.positionGeodetic = {'value': [self.LATITUDE, self.LONGITUDE, self.ALTITUDE], 'valid': position_valid}
location.calibratedOrientationNED = {'value': [0.0, 0.0, math.radians(self.BEARING)], 'valid': True}
return msg
def send_location(self, has_fix: bool) -> None:
def send_location(self, status: log.LiveLocationKalman.Status, position_valid: bool) -> None:
# the first exchange synchronizes the PUB/SUB connection, the second one is actually received
self.pm.send('gpsLocationSP', self.location_message(has_fix))
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
self.map_data.sm.update(1000)
self.pm.send('gpsLocationSP', self.location_message(has_fix))
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
self.map_data.sm.update(1000)
def read_position(self) -> dict:
@@ -46,7 +50,7 @@ class TestOsmMapData(OpenpilotTestCase):
return json.loads(value)
def test_writes_position_for_mapd(self):
self.send_location(True)
self.send_location(log.LiveLocationKalman.Status.valid, True)
self.map_data.tick()
position = self.read_position()
@@ -55,10 +59,10 @@ class TestOsmMapData(OpenpilotTestCase):
self.assertAlmostEqual(position['bearing'], self.BEARING, delta=1e-6)
def test_invalid_localizer_keeps_last_known_position(self):
self.send_location(True)
self.send_location(log.LiveLocationKalman.Status.valid, True)
self.map_data.tick()
self.send_location(False)
self.send_location(log.LiveLocationKalman.Status.uninitialized, False)
self.map_data.tick()
position = self.read_position()
@@ -0,0 +1,3 @@
params_learner
paramsd
locationd
@@ -0,0 +1,30 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
live_ekf = env.RednoseCompileFilter(
target='live',
filter_gen_script='models/live_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['live_kf_constants.h'],
gen_script_deps=rednose_gen_deps,
)
# locationd build
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
lenv.Depends(locationd, rednose)
lenv.Depends(locationd, live_ekf)
@@ -1,181 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import time
from collections import deque
import numpy as np
import openpilot.cereal.messaging as messaging
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.common.transformations.orientation import rot_from_euler
EARTH_RADIUS = 6378137.0
GPS_LATENCY = {'gpsLocation': 0.630, 'gpsLocationExternal': 0.095} # s, empirical fix latency per receiver
RESET_DISTANCE = 100.0 # m
MAX_REJECTS = 5
MAX_POS_STD = 50.0 # m
MIN_BEARING_SPEED = 5.0 # m/s
MAX_BEARING_ACCURACY = 10.0 # deg
GPS_TIMEOUT = 2.0 # s
SEED_INTERVAL = 60.0 # s
def wrap(a: float) -> float:
return (a + math.pi) % (2 * math.pi) - math.pi
class GpsFuser:
def __init__(self):
self.t: float | None = None
self.lat = self.lon = self.heading = math.nan
self.pos_var = self.heading_var = math.inf
self.history: deque = deque(maxlen=40)
self.rejects = 0
@property
def initialized(self) -> bool:
return math.isfinite(self.lat)
@property
def valid(self) -> bool:
return self.initialized and math.isfinite(self.heading) and self.pos_var < MAX_POS_STD ** 2
def _move(self, north: float, east: float) -> None:
self.lat += math.degrees(north / EARTH_RADIUS)
self.lon += math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(self.lat))))
@staticmethod
def _offset(lat: float, lon: float, ref_lat: float, ref_lon: float) -> tuple[float, float]:
north = math.radians(lat - ref_lat) * EARTH_RADIUS
east = math.radians(lon - ref_lon) * EARTH_RADIUS * math.cos(math.radians(ref_lat))
return north, east
def _state_at(self, t: float) -> tuple[float, float, float]:
for ht, lat, lon, heading in reversed(self.history):
if ht <= t:
return lat, lon, heading
return self.lat, self.lon, self.heading
def predict(self, t: float, speed: float, yaw_rate: float) -> None:
dt = 0.0 if self.t is None else t - self.t
self.t = t
if not 0.0 < dt < 1.0 or not self.initialized:
return
if math.isfinite(self.heading):
self.heading = (self.heading + yaw_rate * dt) % (2 * math.pi)
self._move(speed * dt * math.cos(self.heading), speed * dt * math.sin(self.heading))
# dead reckoning error grows with distance driven (speed scale and heading error), not as a random walk
drift = (0.03 + math.sqrt(min(self.heading_var, 1.0))) * abs(speed) * dt
self.pos_var = (math.sqrt(self.pos_var) + drift) ** 2 + 0.01 * dt
self.heading_var += 1e-5 * dt
self.history.append((t, self.lat, self.lon, self.heading))
def update_gps(self, t: float, gps, latency: float) -> None:
# qcom reports no horizontal accuracy
accuracy = gps.horizontalAccuracy if gps.horizontalAccuracy > 0 else math.sqrt(3) * gps.verticalAccuracy
if not gps.hasFix or not 0 < accuracy < 1500 or gps.speedAccuracy <= 0:
return
if abs(gps.latitude) > 90 or abs(gps.longitude) > 180:
return
then_lat, then_lon, then_heading = self._state_at(t - latency)
moved = self._offset(self.lat, self.lon, then_lat, then_lon) if self.initialized else (0.0, 0.0)
bearing = math.radians(gps.bearingDeg)
bearing_ok = gps.speed > MIN_BEARING_SPEED and 0 < gps.bearingAccuracyDeg < MAX_BEARING_ACCURACY
pos_r = accuracy ** 2
innov = self._offset(gps.latitude, gps.longitude, then_lat, then_lon) if self.initialized else (math.inf, math.inf)
dist = math.hypot(*innov)
if not self.initialized or dist > RESET_DISTANCE or self.rejects >= MAX_REJECTS:
self.lat, self.lon = gps.latitude, gps.longitude
self._move(*moved)
self.pos_var = pos_r
self.rejects = 0
self.history.clear()
if bearing_ok:
self.heading, self.heading_var = bearing, math.radians(gps.bearingAccuracyDeg) ** 2
return
s = self.pos_var + pos_r
if dist ** 2 > 9.0 * s and dist > 10.0:
self.rejects += 1
return
self.rejects = 0
k = self.pos_var / s
lat0, lon0 = self.lat, self.lon
self._move(k * innov[0], k * innov[1])
dlat, dlon = self.lat - lat0, self.lon - lon0
self.history = deque(((ht, lat + dlat, lon + dlon, h) for ht, lat, lon, h in self.history), maxlen=self.history.maxlen)
self.pos_var *= 1 - k
if bearing_ok:
head_r = math.radians(gps.bearingAccuracyDeg) ** 2
if not math.isfinite(self.heading):
self.heading, self.heading_var = bearing, head_r
else:
kh = self.heading_var / (self.heading_var + head_r)
self.heading = (self.heading + kh * wrap(bearing - then_heading)) % (2 * math.pi)
self.heading_var *= 1 - kh
def main():
params = Params()
gps_service = get_gps_location_service(params)
latency = GPS_LATENCY[gps_service]
sm = messaging.SubMaster(['deviceMotion', 'carState', 'extrinsicsCalibration', gps_service], poll='deviceMotion')
pm = messaging.PubMaster(['gpsLocationSP'])
fuser = GpsFuser()
calib_from_device = np.eye(3)
last_seed = 0.0
while True:
sm.update()
t = sm.logMonoTime['deviceMotion'] * 1e-9
rpy = list(sm['extrinsicsCalibration'].rpyCalib)
if sm.updated['extrinsicsCalibration'] and len(rpy) == 3:
calib_from_device = rot_from_euler(rpy).T
gps = sm[gps_service]
gps_t = sm.logMonoTime[gps_service] * 1e-9
if sm.updated[gps_service] and sm.valid[gps_service]:
fuser.update_gps(gps_t, gps, latency)
if sm.valid['deviceMotion']:
w = sm['deviceMotion'].angularVelocityDevice
yaw_rate = float((calib_from_device @ np.array([w.x, w.y, w.z]))[2])
fuser.predict(t, sm['carState'].vEgo, yaw_rate)
gps_ok = gps.hasFix and 0 <= t - gps_t < GPS_TIMEOUT
msg = messaging.new_message('gpsLocationSP', valid=fuser.valid or gps_ok)
out = msg.gpsLocationSP
out.gpsOK = gps_ok
out.fused = fuser.valid
if fuser.valid:
out.hasFix = True
out.latitude, out.longitude = fuser.lat, fuser.lon
out.bearingDeg = math.degrees(fuser.heading) % 360
out.bearingAccuracyDeg = math.degrees(math.sqrt(fuser.heading_var))
out.horizontalAccuracy = math.sqrt(fuser.pos_var)
out.speed = sm['carState'].vEgo
elif gps_ok:
out.hasFix = True
out.latitude, out.longitude = gps.latitude, gps.longitude
out.bearingDeg, out.bearingAccuracyDeg = gps.bearingDeg, gps.bearingAccuracyDeg
out.horizontalAccuracy, out.speed = gps.horizontalAccuracy, gps.speed
pm.send('gpsLocationSP', msg)
if fuser.valid and time.monotonic() - last_seed > SEED_INTERVAL:
params.put("LastGPSPositionFused", json.dumps({"latitude": fuser.lat, "longitude": fuser.lon}))
last_seed = time.monotonic()
if __name__ == "__main__":
main()
@@ -0,0 +1,750 @@
#include "sunnypilot/selfdrive/locationd/locationd.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
const double RESET_TRACKER_DECAY = 0.99995;
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
const double MAX_FILTER_REWIND_TIME = 0.8; // s
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
res[i] = floatlist[i];
}
return res;
}
static Vector4d quat2vector(const Quaterniond& quat) {
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}
static Quaterniond vector2quat(const VectorXd& vec) {
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
}
static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
meas.setValue(kj::arrayPtr(val.data(), val.size()));
meas.setStd(kj::arrayPtr(std.data(), std.size()));
meas.setValid(valid);
}
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
return ((rot_matrix * cov_in) * rot_matrix.transpose());
}
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
// Stds cannot be rotated like values, only covariances can be rotated
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}
Localizer::Localizer(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
this->calib = Vector3d(0.0, 0.0, 0.0);
this->device_from_calib = MatrixXdr::Identity(3, 3);
this->calib_from_device = MatrixXdr::Identity(3, 3);
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
this->posenet_stds.push_back(10.0);
}
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P();
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
VectorXd vel_device = device_from_ecef * vel_ecef;
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
H_input << device_from_ecef_eul, vel_ecef;
MatrixXdr HH = this->kf->H(H_input);
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
VectorXd vel_calib = this->calib_from_device * vel_device;
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
Vector3d nans = Vector3d(NAN, NAN, NAN);
// TODO fill in NED and Calibrated stds
// write measurements to msg
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
}
double old_mean = 0.0, new_mean = 0.0;
int i = 0;
for (double x : this->posenet_stds) {
if (i < POSENET_STD_HIST_HALF) {
old_mean += x;
} else {
new_mean += x;
}
i++;
}
old_mean /= POSENET_STD_HIST_HALF;
new_mean /= POSENET_STD_HIST_HALF;
// experimentally found these values, no false positives in 20k minutes of driving
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
fix.setDeviceStable(!this->device_fell);
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
this->device_fell = false;
//fix.setGpsWeek(this->time.week);
//fix.setGpsTimeOfWeek(this->time.tow);
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
fix.setTimeSinceReset(time_since_reset);
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
} else {
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
}
}
VectorXd Localizer::get_position_geodetic() {
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
}
VectorXd Localizer::get_state() {
return this->kf->get_x();
}
VectorXd Localizer::get_stdev() {
return this->kf->get_P().diagonal().array().sqrt();
}
bool Localizer::are_inputs_ok() {
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
}
void Localizer::observation_timings_invalid_reset(){
this->observation_timings_invalid = false;
}
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
// TODO does not yet account for double sensor readings in the log
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (log.getTimestamp() == 0) {
return;
}
double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
this->observation_timings_invalid = true;
return;
} else if (!this->is_timestamp_valid(sensor_time)) {
this->observation_timings_invalid = true;
return;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (log.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
this->observation_values_invalid["gyroscope"] *= DECAY;
} else {
this->observation_values_invalid["gyroscope"] += 1.0;
}
}
// Accelerometer
if (log.which() == cereal::SensorEventData::ACCELERATION) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
this->observation_values_invalid["accelerometer"] *= DECAY;
} else {
this->observation_values_invalid["accelerometer"] += 1.0;
}
}
}
void Localizer::input_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> observe current obs with reasonable STD
this->kf->predict(current_time);
VectorXd current_x = this->kf->get_x();
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
//this->gps_valid = false;
this->determine_gps_mode(current_time);
return;
}
double sensor_time = current_time - sensor_time_offset;
// Process message
//this->gps_valid = true;
this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
this->determine_gps_mode(current_time);
return;
}
double sensor_time = log.getMeasTime() * 1e-9;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
this->orientation_reset_count++;
} else {
this->orientation_reset_count = 0;
}
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
// always reset on first gps message and if the location is off but the accuracy is high
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
this->orientation_reset_count = 0;
}
this->gps_mode = true;
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
this->car_speed = std::abs(log.getVEgo());
this->standstill = log.getStandstill();
if (this->standstill) {
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_calib_std[0]);
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
trans_calib_std *= 10.0;
rot_calib_std *= 10.0;
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
{ trans_device }, { trans_device_cov });
this->observation_values_invalid["cameraOdometry"] *= DECAY;
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
}
void Localizer::handle_live_calib(double current_time, const cereal::ExtrinsicsCalibration::Reader& log) {
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if (log.getRpyCalib().size() > 0) {
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
return;
}
this->calib = live_calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == cereal::ExtrinsicsCalibration::Status::CALIBRATED;
this->observation_values_invalid["extrinsicsCalibration"] *= DECAY;
}
}
void Localizer::reset_kalman(double current_time) {
const VectorXd &init_x = this->kf->get_initial_x();
const MatrixXdr &init_P = this->kf->get_initial_P();
this->reset_kalman(current_time, init_x, init_P);
}
void Localizer::finite_check(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
if (!all_finite) {
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::time_check(double current_time) {
if (std::isnan(this->last_reset_time)) {
this->last_reset_time = current_time;
}
if (std::isnan(this->first_valid_log_time)) {
this->first_valid_log_time = current_time;
}
double filter_time = this->kf->get_filter_time();
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap) {
LOGE("Time gap of over 10s detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::update_reset_tracker() {
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
if (this->is_gps_ok()) {
this->reset_tracker *= RESET_TRACKER_DECAY;
} else {
this->reset_tracker = 0.0;
}
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
// too nonlinear to init on completely wrong
VectorXd current_x = this->kf->get_x();
MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
this->reset_kalman(current_time, current_x, init_P);
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
}
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
this->handle_msg(event);
}
void Localizer::handle_msg(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->time_check(t);
if (log.isAccelerometer()) {
this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->handle_cam_odo(t, log.getCameraOdometry());
} else if (log.isExtrinsicsCalibration()) {
this->handle_live_calib(t, log.getExtrinsicsCalibration());
}
this->finite_check();
this->update_reset_tracker();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
this->build_live_location(liveLoc);
liveLoc.setSensorsOK(sensorsOK);
liveLoc.setGpsOK(gpsOK);
liveLoc.setInputsOK(inputsOK);
return msg_builder.toBytes();
}
bool Localizer::is_gps_ok() {
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
}
}
return true;
}
bool Localizer::is_timestamp_valid(double current_time) {
double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
return false;
}
return true;
}
void Localizer::determine_gps_mode(double current_time) {
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
if (this->gps_mode){
this->gps_mode = false;
this->reset_kalman(current_time);
} else {
this->input_fake_gps_observations(current_time);
}
}
}
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
Params params;
LocalizerGnssSource source;
const char* gps_location_socket;
if (params.getBool("UbloxAvailable")) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "extrinsicsCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "extrinsicsCalibration", "accelerometer", "gyroscope"};
for (std::string service : critical_input_services) {
this->observation_values_invalid.insert({service, 0.0});
}
while (!do_exit) {
sm.update();
if (filterInitialized){
this->observation_timings_invalid_reset();
for (const char* service : service_list) {
if (sm.updated(service) && sm.valid(service)){
const cereal::Event::Reader log = sm[service];
this->handle_msg(log);
}
}
} else {
filterInitialized = sm.allAliveAndValid();
}
const char* trigger_msg = "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allValid() && this->are_inputs_ok();
bool gpsOK = this->is_gps_ok();
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
// Log time to first fix
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
}
MessageBuilder msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
if (cnt % 1200 == 0 && gpsOK) { // once a minute
VectorXd posGeo = this->get_position_geodetic();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON);
}
cnt++;
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
}
@@ -0,0 +1,100 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include <deque>
#include <fstream>
#include <memory>
#include <map>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "sunnypilot/common/transformations/coordinates.hpp"
#include "sunnypilot/common/transformations/orientation.hpp"
#include "sunnypilot/system/sensord/sensors/constants.h"
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
#define VISION_DECIMATION 2
#define SENSOR_DECIMATION 10
#define POSENET_STD_HIST_HALF 20
enum LocalizerGnssSource {
UBLOX, QCOM
};
class Localizer {
public:
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
int locationd_thread();
void reset_kalman(double current_time = NAN);
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
void finite_check(double current_time = NAN);
void time_check(double current_time = NAN);
void update_reset_tracker();
bool is_gps_ok();
bool critical_services_valid(const std::map<std::string, double> &critical_services);
bool is_timestamp_valid(double current_time);
void determine_gps_mode(double current_time);
bool are_inputs_ok();
void observation_timings_invalid_reset();
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
Eigen::VectorXd get_position_geodetic();
Eigen::VectorXd get_state();
Eigen::VectorXd get_stdev();
void handle_msg_bytes(const char *data, const size_t size);
void handle_msg(const cereal::Event::Reader& log);
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
void handle_live_calib(double current_time, const cereal::ExtrinsicsCalibration::Reader& log);
void input_fake_gps_observations(double current_time);
private:
std::unique_ptr<LiveKalman> kf;
Eigen::VectorXd calib;
MatrixXdr device_from_calib;
MatrixXdr calib_from_device;
bool calibrated = false;
double car_speed = 0.0;
double last_reset_time = NAN;
std::deque<double> posenet_stds;
std::unique_ptr<LocalCoord> converter;
int64_t unix_timestamp_millis = 0;
double reset_tracker = 0.0;
bool device_fell = false;
bool gps_mode = false;
double first_valid_log_time = NAN;
double ttff = NAN;
double last_gps_msg = 0;
LocalizerGnssSource gnss_source;
bool observation_timings_invalid = false;
std::map<std::string, double> observation_values_invalid;
bool standstill = true;
int32_t orientation_reset_count = 0;
float gps_std_factor;
float gps_variance_factor;
float gps_vertical_variance_factor;
double gps_time_offset;
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
void configure_gnss_source(const LocalizerGnssSource &source);
};
@@ -0,0 +1 @@
generated/
+180
View File
@@ -0,0 +1,180 @@
#!/usr/bin/env python3
import math
import sys
from typing import Any
import numpy as np
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
from openpilot.common.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
i = 0
def _slice(n):
global i
s = slice(i, i + n)
i += n
return s
class States:
# Vehicle model params
STIFFNESS = _slice(1) # [-]
STEER_RATIO = _slice(1) # [-]
ANGLE_OFFSET = _slice(1) # [rad]
ANGLE_OFFSET_FAST = _slice(1) # [rad]
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
class CarKalman(KalmanFilter):
name = 'car'
initial_x = np.array([
1.0,
15.0,
0.0,
0.0,
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
Q = np.diag([
(.05 / 100)**2,
.01**2,
math.radians(0.02)**2,
math.radians(0.25)**2,
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
math.radians(1)**2,
])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
global_vars = [
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
@staticmethod
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
# Linearized single-track lateral dynamics, equations 7.211-7.213
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
# globals
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":
generated_dir = sys.argv[2]
CarKalman.generate_code(generated_dir)
@@ -0,0 +1,92 @@
import os
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]
@@ -0,0 +1,122 @@
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
LiveKalman::LiveKalman() {
this->dim_state = live_initial_x.rows();
this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// 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<int>{3}, std::vector<std::string>(), 0.8);
}
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
VectorXd LiveKalman::get_x() {
return this->filter->state();
}
MatrixXdr LiveKalman::get_P() {
return this->filter->covs();
}
double LiveKalman::get_filter_time() {
return this->filter->get_filter_time();
}
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
}
void LiveKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &LiveKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &LiveKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
MatrixXdr LiveKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}
@@ -0,0 +1,66 @@
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/live_kf_constants.h"
#include "rednose/helpers/ekf_sym.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
class LiveKalman {
public:
LiveKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "live";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};
+242
View File
@@ -0,0 +1,242 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
import sympy as sp
import inspect
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def numpy2eigenstring(arr):
assert(len(arr.shape) == 1)
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
class States:
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ACCELERATION_ERR = slice(15, 18)
ACC_BIAS_ERR = slice(18, 21)
class LiveKalman:
name = 'live'
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10**2, 10**2, 10**2,
0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# state covariance when resetting midway in a segment
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@staticmethod
def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
# write constants to extra header file for use in cpp
live_kf_header = "#pragma once\n\n"
live_kf_header += "#include <unordered_map>\n"
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
assert(slc.step is None) # unsupported
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
live_kf_header += "\n"
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
live_kf_header += "\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
live_kf_header += "};\n\n"
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
if __name__ == "__main__":
generated_dir = sys.argv[2]
LiveKalman.generate_code(generated_dir)
@@ -0,0 +1 @@
out/
@@ -1,73 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import math
from types import SimpleNamespace
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.selfdrive.locationd.gps_fuser import EARTH_RADIUS, GPS_LATENCY, GpsFuser
LAT0, LON0 = 39.0, -77.0
LATENCY = GPS_LATENCY['gpsLocation']
def fix(north, east, speed=30.0, bearing=90.0, h_acc=0.0, v_acc=1.5):
return SimpleNamespace(latitude=LAT0 + math.degrees(north / EARTH_RADIUS),
longitude=LON0 + math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(LAT0)))),
speed=speed, bearingDeg=bearing, horizontalAccuracy=h_acc, verticalAccuracy=v_acc,
speedAccuracy=0.3, bearingAccuracyDeg=0.5, hasFix=True)
def position(f: GpsFuser) -> tuple[float, float]:
north = math.radians(f.lat - LAT0) * EARTH_RADIUS
east = math.radians(f.lon - LON0) * EARTH_RADIUS * math.cos(math.radians(LAT0))
return north, east
class TestGpsFuser(OpenpilotTestCase):
def drive(self, f, seconds, speed=30.0, start=0.0, gps=lambda t, true_east: fix(0.0, true_east)):
t = start
while t < start + seconds:
t = round(t + 0.05, 2)
f.predict(t, speed, 0.0)
if abs(t - round(t)) < 1e-6:
f.update_gps(t, gps(t, speed * (t - LATENCY)), LATENCY)
return t
def test_compensates_latency(self):
f = GpsFuser()
t = self.drive(f, 30)
self.assertTrue(f.valid)
north, east = position(f)
self.assertAlmostEqual(east, 30.0 * t, delta=3.0)
self.assertAlmostEqual(north, 0.0, delta=1.0)
def test_rejects_single_jump(self):
f = GpsFuser()
t = self.drive(f, 20)
f.predict(t + 0.05, 30.0, 0.0)
f.update_gps(t + 0.05, fix(40.0, 30.0 * (t + 0.05 - LATENCY)), LATENCY)
self.assertAlmostEqual(position(f)[0], 0.0, delta=2.0)
def test_follows_persistent_offset(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 10, start=t, gps=lambda _, e: fix(150.0, e))
self.assertAlmostEqual(position(f)[0], 150.0, delta=3.0)
def test_ignores_bearing_when_slow(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 5, speed=1.0, start=t, gps=lambda _, e: fix(0.0, e, speed=1.0, bearing=270.0))
self.assertAlmostEqual(math.degrees(f.heading), 90.0, delta=1.0)
def test_invalid_after_long_outage(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 120, start=t, gps=lambda _, e: SimpleNamespace(hasFix=False, horizontalAccuracy=0.0, verticalAccuracy=0.0,
speedAccuracy=0.0, latitude=0.0, longitude=0.0))
self.assertFalse(f.valid)
@@ -0,0 +1,96 @@
import platform
import unittest
import json
import random
import time
import capnp
import openpilot.cereal.messaging as messaging
from openpilot.cereal.services import SERVICE_LIST
from openpilot.common.params import Params
from openpilot.common.transformations.coordinates import ecef2geodetic
from openpilot.system.manager.process_config import managed_processes
from openpilot.common.test import OpenpilotTestCase
@unittest.skipIf(platform.system() == 'Darwin', "msgq unsupported on macOS")
class TestLocationdProc(OpenpilotTestCase):
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'extrinsicsCalibration',
'accelerometer', 'gyroscope']
def setup_method(self):
self.pm = messaging.PubMaster(self.LLD_MSGS)
self.params = Params()
self.params.put_bool("UbloxAvailable", True)
managed_processes['locationd_llk'].start()
def teardown_method(self):
managed_processes['locationd_llk'].stop()
def get_msg(self, name, t):
try:
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal":
msg.gpsLocationExternal.flags = 1
msg.gpsLocationExternal.hasFix = True
msg.gpsLocationExternal.verticalAccuracy = 1.0
msg.gpsLocationExternal.speedAccuracy = 1.0
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
msg.gpsLocationExternal.latitude = float(self.lat)
msg.gpsLocationExternal.longitude = float(self.lon)
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = float(self.alt)
#if name == "gnssMeasurements":
# msg.gnssMeasurements.measTime = t
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
# msg.gnssMeasurements.positionECEF.valid = True
# msg.gnssMeasurements.velocityECEF.value = []
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
# msg.gnssMeasurements.velocityECEF.valid = True
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):
random.seed(123489234)
self.params.remove('LastGPSPositionLLK')
self.x = -2710700 + (random.random() * 1e5)
self.y = -4280600 + (random.random() * 1e5)
self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
# get fake messages at the correct frequency, listed in services.py
msgs = []
for sec in range(65):
for name in self.LLD_MSGS:
for j in range(int(SERVICE_LIST[name].frequency)):
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
self.pm.send(msg.which(), msg)
if msg.which() == "cameraOdometry":
self.pm.wait_for_readers_to_update(msg.which(), timeout=1, dt=0.005)
for _ in range(50):
val = self.params.get('LastGPSPositionLLK')
if val is not None:
break
time.sleep(0.1)
self.assertIsNotNone(val, "LastGPSPositionLLK not written within 5s")
lastGPS = json.loads(val)
self.assertAlmostEqual(lastGPS['latitude'], self.lat, delta=0.001)
self.assertAlmostEqual(lastGPS['longitude'], self.lon, delta=0.001)
self.assertAlmostEqual(lastGPS['altitude'], self.alt, delta=0.001)
+2 -1
View File
@@ -184,7 +184,8 @@ procs += [
NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
PythonProcess("mapd_manager", "openpilot.sunnypilot.mapd.mapd_manager", always_run),
PythonProcess("gps_fuser", "openpilot.sunnypilot.selfdrive.locationd.gps_fuser", only_onroad),
# locationd
NativeProcess("locationd_llk", "openpilot/sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
]
if os.path.exists("../../sunnypilot/sunnylink/uploader.py"):