310 lines
9.5 KiB
Plaintext
310 lines
9.5 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# In this example we will show the difference between fixes computed with laika\n",
|
|
"# from raw data of the ublox receiver vs the the fixes the ublox receiver\n",
|
|
"# computes"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 2,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"from pathlib import Path\n",
|
|
"\n",
|
|
"base_dir = Path('example_data')\n",
|
|
"raw_ublox_t = np.load(base_dir / 'raw_gnss_ublox/t')\n",
|
|
"raw_ublox = np.load(base_dir / 'raw_gnss_ublox/value')\n",
|
|
"fixes_ublox_t = np.load(base_dir / 'live_gnss_ublox/t')\n",
|
|
"fixes_ublox = np.load(base_dir / 'live_gnss_ublox/value')"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# We get the raw data into our format from the log array format\n",
|
|
"\n",
|
|
"from laika.raw_gnss import normal_meas_from_array\n",
|
|
"\n",
|
|
"measurements = np.array([normal_meas_from_array(arr) for arr in raw_ublox])"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 4,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# initialize an astrodog with dgps corrections\n",
|
|
"\n",
|
|
"from laika import AstroDog\n",
|
|
"\n",
|
|
"dog = AstroDog(dgps=True)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 5,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": [
|
|
"PosixPath('/tmp/gnss/cors_coord/cors_station_positions')"
|
|
]
|
|
},
|
|
"execution_count": 5,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"# Building this cache takes forever, just copy it from repo\n",
|
|
"\n",
|
|
"from shutil import copyfile\n",
|
|
"\n",
|
|
"cache_directory = Path(dog.cache_dir) / 'cors_coord'\n",
|
|
"cache_directory.mkdir(parents=True, exist_ok=True)\n",
|
|
"copyfile('cors_station_positions', cache_directory / 'cors_station_positions')"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 6,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Downloading https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/18213/final/Sta20123.sp3\n",
|
|
"Downloading https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/18214/final/Sta20124.sp3\n",
|
|
"Downloading https://github.com/commaai/gnss-data-alt/raw/master/MCC/PRODUCTS/18215/final/Sta20125.sp3\n",
|
|
"Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/2012/igs20123.sp3.Z\n",
|
|
"Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/2012/igs20124.sp3.Z\n",
|
|
"Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/2012/igs20125.sp3.Z\n"
|
|
]
|
|
},
|
|
{
|
|
"data": {
|
|
"application/vnd.jupyter.widget-view+json": {
|
|
"model_id": "7e625de4e34e44f0922ead82c10c26e0",
|
|
"version_major": 2,
|
|
"version_minor": 0
|
|
},
|
|
"text/plain": [
|
|
" 0%| | 0/600 [00:00<?, ?it/s]"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
},
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/pbl1/pbl12140.18d.gz\n",
|
|
"HTTPS error 404\n",
|
|
"Downloading https://alt.ngs.noaa.gov/corsdata/rinex/2018/214/pbl1/pbl12140.18d.gz\n",
|
|
"HTTPS error 404\n",
|
|
"File not downloaded, check availability on server.\n",
|
|
"Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/pbl2/pbl22140.18d.gz\n",
|
|
"HTTPS error 404\n",
|
|
"Downloading https://alt.ngs.noaa.gov/corsdata/rinex/2018/214/pbl2/pbl22140.18d.gz\n",
|
|
"HTTPS error 404\n",
|
|
"File not downloaded, check availability on server.\n",
|
|
"Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/hsib/hsib2140.18d.gz\n",
|
|
"HTTPS error 404\n",
|
|
"Downloading https://alt.ngs.noaa.gov/corsdata/rinex/2018/214/hsib/hsib2140.18d.gz\n",
|
|
"HTTPS error 404\n",
|
|
"File not downloaded, check availability on server.\n",
|
|
"Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/tibb/tibb2140.18d.gz\n",
|
|
"Downloading https://geodesy.noaa.gov/corsdata/rinex/2018/214/capo/capo2140.18d.gz\n",
|
|
"Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/ionex/2018/214/codg2140.18i.Z\n",
|
|
"Downloading https://github.com/commaai/gnss-data/raw/master/gnss/products/bias/2018/CAS0MGXRAP_20182140000_01D_01D_DCB.BSX.gz\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"from laika.raw_gnss import process_measurements, correct_measurements\n",
|
|
"from laika.opt import calc_pos_fix\n",
|
|
"from tqdm.auto import tqdm\n",
|
|
"\n",
|
|
"# We want to group measurements by measurement epoch\n",
|
|
"# this makes the kalman filter faster and is easier\n",
|
|
"# to reason about\n",
|
|
"grouped_t = np.unique(raw_ublox_t)\n",
|
|
"grouped_meas_processed = []\n",
|
|
"corrected_meas_arrays = []\n",
|
|
"\n",
|
|
"# process measurement groups\n",
|
|
"for t in grouped_t:\n",
|
|
" meas = measurements[raw_ublox_t == t]\n",
|
|
" grouped_meas_processed.append(process_measurements(meas, dog))\n",
|
|
"\n",
|
|
"# correct measurement groups with an estimate position\n",
|
|
"# that was computes with weighted-least-squares on\n",
|
|
"# the first epoch\n",
|
|
"# WARNING: can take up to 10min\n",
|
|
"wls_estimate = calc_pos_fix(grouped_meas_processed[0])\n",
|
|
"est_pos = wls_estimate[0][:3]\n",
|
|
"for proc in tqdm(grouped_meas_processed):\n",
|
|
" corrected = correct_measurements(proc, est_pos, dog)\n",
|
|
" corrected_meas_arrays.append(np.array([c.as_array() for c in corrected]))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 7,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"application/vnd.jupyter.widget-view+json": {
|
|
"model_id": "8b1fa61873754be19c7f3e9d32d880f8",
|
|
"version_major": 2,
|
|
"version_minor": 0
|
|
},
|
|
"text/plain": [
|
|
" 0%| | 0/600 [00:00<?, ?it/s]"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"for proc in tqdm(grouped_meas_processed):\n",
|
|
" corrected = correct_measurements(proc, est_pos, dog)\n",
|
|
" corrected_meas_arrays.append(np.array([c.as_array() for c in corrected]))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 8,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# Generate and build a CFFI library from the Kalman filter defined symbolically in SymPy\n",
|
|
"from kalman.models.gnss_kf import GNSSKalman\n",
|
|
"\n",
|
|
"GNSSKalman.generate_code()\n",
|
|
"!g++ kalman/generated/gnss.cpp -Wall -fPIC -shared -o kalman/generated/libgnss.so"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 9,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"application/vnd.jupyter.widget-view+json": {
|
|
"model_id": "473c7197f3f84f01b41d672c5c030eba",
|
|
"version_major": 2,
|
|
"version_minor": 0
|
|
},
|
|
"text/plain": [
|
|
" 0%| | 0/1200 [00:00<?, ?it/s]"
|
|
]
|
|
},
|
|
"metadata": {},
|
|
"output_type": "display_data"
|
|
}
|
|
],
|
|
"source": [
|
|
"# We run the Kalman filter\n",
|
|
"\n",
|
|
"from kalman.kalman_helpers import run_car_ekf_offline, ObservationKind\n",
|
|
"\n",
|
|
"ekf = GNSSKalman()\n",
|
|
"init_state = ekf.x\n",
|
|
"init_state[:3] = est_pos\n",
|
|
"ekf.init_state(init_state)\n",
|
|
"ekf_data = {}\n",
|
|
"ekf_data[ObservationKind.PSEUDORANGE_GPS] = (grouped_t, corrected_meas_arrays)\n",
|
|
"ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = (grouped_t, corrected_meas_arrays)\n",
|
|
"ekf_outputs = run_car_ekf_offline(ekf, ekf_data)\n",
|
|
"\n",
|
|
"import laika.lib.coordinates as coord\n",
|
|
"\n",
|
|
"laika_positions_t = ekf_outputs[4]\n",
|
|
"laika_positions_ecef = ekf_outputs[0][:, :3]\n",
|
|
"laika_positions_geodetic = coord.ecef2geodetic(laika_positions_ecef)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 10,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"ublox_positions_geodetic = fixes_ublox[:, [0, 1, 4]]"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 11,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"# By looking at the map, we can see that the two paths compared.\n",
|
|
"# If you want to regenerate the gmplot you will need a google\n",
|
|
"# maps API key\n",
|
|
"\n",
|
|
"import gmplot\n",
|
|
"\n",
|
|
"gmap = gmplot.GoogleMapPlotter(*laika_positions_geodetic[0])\n",
|
|
"#gmap.apikey='...'\n",
|
|
"gmap.plot([x[0] for x in laika_positions_geodetic], [x[1] for x in laika_positions_geodetic], 'blue', edge_width=5)\n",
|
|
"gmap.plot([x[0] for x in ublox_positions_geodetic], [x[1] for x in ublox_positions_geodetic], 'red', edge_width=5)\n",
|
|
"gmap.draw(\"laika_quality_check.html\")\n",
|
|
"\n",
|
|
"import webbrowser\n",
|
|
"import os\n",
|
|
"\n",
|
|
"webbrowser.open('file://' + os.path.realpath(\"laika_quality_check.html\"));"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": null,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": []
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "Python 3 (ipykernel)",
|
|
"language": "python",
|
|
"name": "python3"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 3
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython3",
|
|
"version": "3.9.7"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|