pygnss 0.0.0__cp312-cp312-musllinux_1_2_i686.whl → 0.1.1__cp312-cp312-musllinux_1_2_i686.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Potentially problematic release.
This version of pygnss might be problematic. Click here for more details.
- pygnss/__init__.py +1 -1
- pygnss/_c_ext/src/hatanaka.c +4 -2
- pygnss/_c_ext.cpython-312-i386-linux-musl.so +0 -0
- pygnss/filter/__init__.py +62 -0
- pygnss/filter/ekf.py +77 -0
- pygnss/filter/models.py +73 -0
- pygnss/filter/ukf.py +319 -0
- pygnss/hatanaka.py +19 -3
- {pygnss-0.0.0.dist-info → pygnss-0.1.1.dist-info}/METADATA +3 -16
- {pygnss-0.0.0.dist-info → pygnss-0.1.1.dist-info}/RECORD +14 -10
- {pygnss-0.0.0.dist-info → pygnss-0.1.1.dist-info}/LICENSE +0 -0
- {pygnss-0.0.0.dist-info → pygnss-0.1.1.dist-info}/WHEEL +0 -0
- {pygnss-0.0.0.dist-info → pygnss-0.1.1.dist-info}/entry_points.txt +0 -0
- {pygnss-0.0.0.dist-info → pygnss-0.1.1.dist-info}/top_level.txt +0 -0
pygnss/__init__.py
CHANGED
|
@@ -1 +1 @@
|
|
|
1
|
-
__version__ = "
|
|
1
|
+
__version__ = "0.1.1"
|
pygnss/_c_ext/src/hatanaka.c
CHANGED
|
@@ -3,7 +3,6 @@
|
|
|
3
3
|
|
|
4
4
|
#include "hatanaka/include/crx2rnx.h"
|
|
5
5
|
|
|
6
|
-
static const int N_FIELDS = 4; // Number of fields for struct gnss_meas
|
|
7
6
|
|
|
8
7
|
static char* get_crx_line(void* _args, size_t n_max, char* dst) {
|
|
9
8
|
|
|
@@ -21,6 +20,8 @@ static bool is_eof(void* _args) {
|
|
|
21
20
|
|
|
22
21
|
static int on_measurement(const struct gnss_meas* gnss_meas, void* _args) {
|
|
23
22
|
|
|
23
|
+
static const int N_FIELDS = 5; // Number of fields for struct gnss_meas
|
|
24
|
+
|
|
24
25
|
int ret = -1;
|
|
25
26
|
PyObject* list = (PyObject*)_args;
|
|
26
27
|
|
|
@@ -41,10 +42,11 @@ static int on_measurement(const struct gnss_meas* gnss_meas, void* _args) {
|
|
|
41
42
|
PyList_SetItem(row, 1, PyUnicode_FromStringAndSize(gnss_meas->satid, 3));
|
|
42
43
|
PyList_SetItem(row, 2, PyUnicode_FromStringAndSize(gnss_meas->rinex3_code, 3));
|
|
43
44
|
PyList_SetItem(row, 3, PyFloat_FromDouble(gnss_meas->value));
|
|
45
|
+
PyList_SetItem(row, 4, PyLong_FromUnsignedLong(gnss_meas->lli));
|
|
44
46
|
|
|
45
47
|
// Add inner lists to the outer list
|
|
46
48
|
PyList_Append(list, row);
|
|
47
|
-
Py_DECREF(row);
|
|
49
|
+
Py_DECREF(row); // Decrement the reference count of 'row'
|
|
48
50
|
|
|
49
51
|
ret = 0;
|
|
50
52
|
exit:
|
|
Binary file
|
|
@@ -0,0 +1,62 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Module for the filter class
|
|
3
|
+
|
|
4
|
+
Some notation conventions:
|
|
5
|
+
|
|
6
|
+
- $x_m$ Predicted state from the previous k-1 state
|
|
7
|
+
- $y_m$ indicates the observations resulted from the predicted
|
|
8
|
+
state ($x_m$)
|
|
9
|
+
- $H$ is the design (Jacobian) matrix, that translates from state to observation
|
|
10
|
+
(i.e. $y = H \\cdot x$)
|
|
11
|
+
- $\\Phi$ is the state transition matrix, that translates from the
|
|
12
|
+
state k-1 to the predicted state ($x_m$)
|
|
13
|
+
"""
|
|
14
|
+
from abc import ABC, abstractmethod
|
|
15
|
+
from collections import namedtuple
|
|
16
|
+
from typing import Tuple
|
|
17
|
+
|
|
18
|
+
import numpy as np
|
|
19
|
+
|
|
20
|
+
ModelObs = namedtuple('ModelObs', ('y_m', 'H')) # y_m must be an array of arrays (2D shaped)
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
class Model(ABC):
|
|
24
|
+
"""
|
|
25
|
+
Abstract class that declares the interface for entities that model
|
|
26
|
+
an entity to be used by an estimation filter
|
|
27
|
+
"""
|
|
28
|
+
|
|
29
|
+
@abstractmethod
|
|
30
|
+
def propagate_state(self, state: np.array) -> np.array:
|
|
31
|
+
"""
|
|
32
|
+
Propagate a state from time k-1 to k
|
|
33
|
+
"""
|
|
34
|
+
|
|
35
|
+
@abstractmethod
|
|
36
|
+
def to_observations(self, state: np.array, compute_jacobian: bool = False) -> ModelObs:
|
|
37
|
+
"""
|
|
38
|
+
Propagate a state to its corresponding modelled observations (i.e.
|
|
39
|
+
compute expected observations/measurements for the input state)
|
|
40
|
+
|
|
41
|
+
:return: a tuple where the first element are the observations and the second
|
|
42
|
+
is the Jacobian matrix (if compute_jacobian is True, otherwise the second
|
|
43
|
+
element will be None)
|
|
44
|
+
"""
|
|
45
|
+
|
|
46
|
+
def Phi(self):
|
|
47
|
+
"""
|
|
48
|
+
Provide with the state transition matrix (also noted F in certain
|
|
49
|
+
Kalman notation)
|
|
50
|
+
"""
|
|
51
|
+
|
|
52
|
+
|
|
53
|
+
class StateHandler(ABC):
|
|
54
|
+
"""
|
|
55
|
+
Abstract class that handles the state generated by UKF
|
|
56
|
+
"""
|
|
57
|
+
|
|
58
|
+
@abstractmethod
|
|
59
|
+
def process_state(self, state: np.array, covariance_matrix: np.array):
|
|
60
|
+
"""
|
|
61
|
+
Process the state and associated covariance_matrix
|
|
62
|
+
"""
|
pygnss/filter/ekf.py
ADDED
|
@@ -0,0 +1,77 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Module for the EKF
|
|
3
|
+
"""
|
|
4
|
+
import logging
|
|
5
|
+
from typing import Tuple
|
|
6
|
+
|
|
7
|
+
import numpy as np
|
|
8
|
+
|
|
9
|
+
from . import StateHandler, Model
|
|
10
|
+
|
|
11
|
+
|
|
12
|
+
class Ekf(object):
|
|
13
|
+
"""
|
|
14
|
+
Extended Kalman Filter (EKF)
|
|
15
|
+
"""
|
|
16
|
+
|
|
17
|
+
def __init__(self,
|
|
18
|
+
x0: np.array,
|
|
19
|
+
P0: np.array,
|
|
20
|
+
Q: np.array,
|
|
21
|
+
model: Model,
|
|
22
|
+
state_handler: StateHandler,
|
|
23
|
+
logger: logging.Logger = logging):
|
|
24
|
+
"""
|
|
25
|
+
Initialize the EKF filter object
|
|
26
|
+
"""
|
|
27
|
+
|
|
28
|
+
self.x = x0
|
|
29
|
+
self.P = P0
|
|
30
|
+
self.Q = Q
|
|
31
|
+
|
|
32
|
+
self.model = model
|
|
33
|
+
self.state_handler = state_handler
|
|
34
|
+
|
|
35
|
+
self.logger = logger
|
|
36
|
+
|
|
37
|
+
self.L = len(self.x)
|
|
38
|
+
|
|
39
|
+
def process(self, y_k: np.array, R: np.array):
|
|
40
|
+
"""
|
|
41
|
+
Process an observation batch
|
|
42
|
+
"""
|
|
43
|
+
|
|
44
|
+
# Time update ----------------------------------------------------------
|
|
45
|
+
x_m, P_m = self._time_update()
|
|
46
|
+
|
|
47
|
+
# Measurement update ---------------------------------------------------
|
|
48
|
+
y_m, H = self.model.to_observations(x_m, compute_jacobian=True)
|
|
49
|
+
|
|
50
|
+
P_yy = H @ P_m @ H.T + R
|
|
51
|
+
P_xy = P_m @ H.T
|
|
52
|
+
|
|
53
|
+
self.x = x_m
|
|
54
|
+
self.P = P_m
|
|
55
|
+
|
|
56
|
+
try:
|
|
57
|
+
K = P_xy @ np.linalg.inv(P_yy) # Calculate Kalman gain
|
|
58
|
+
|
|
59
|
+
self.x = self.x + K @ (y_k - y_m) # Update state estimate
|
|
60
|
+
self.P = self.P - K @ H @ P_m # Update covariance estimate
|
|
61
|
+
|
|
62
|
+
except np.linalg.LinAlgError as e:
|
|
63
|
+
self.logger.warning(f'Unable to compute state, keeping previous one. Error: {e}')
|
|
64
|
+
|
|
65
|
+
self.state_handler.process_state(self.x, self.P)
|
|
66
|
+
|
|
67
|
+
def _time_update(self) -> Tuple[np.array, np.array]:
|
|
68
|
+
"""
|
|
69
|
+
Perform a time update step
|
|
70
|
+
"""
|
|
71
|
+
|
|
72
|
+
Phi = self.model.Phi
|
|
73
|
+
|
|
74
|
+
x_m = self.model.propagate_state(self.x)
|
|
75
|
+
P_m = Phi @ self.P @ Phi.T + self.Q
|
|
76
|
+
|
|
77
|
+
return x_m, P_m
|
pygnss/filter/models.py
ADDED
|
@@ -0,0 +1,73 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
|
|
3
|
+
from . import Model, ModelObs
|
|
4
|
+
|
|
5
|
+
|
|
6
|
+
class RangePositioning2D(Model):
|
|
7
|
+
"""
|
|
8
|
+
Basic 2D range-based positioning model
|
|
9
|
+
"""
|
|
10
|
+
|
|
11
|
+
def __init__(self, Phi: np.array, nodes: np.array):
|
|
12
|
+
"""
|
|
13
|
+
Instantiate a RangePositioning2D
|
|
14
|
+
|
|
15
|
+
:param Phi: a 2 x 2 matrix that propagates the state from k-1 to k
|
|
16
|
+
:param nodes: list of nodes of the positioning system, from which the
|
|
17
|
+
range will be computed
|
|
18
|
+
"""
|
|
19
|
+
self._Phi = Phi
|
|
20
|
+
self.nodes = nodes
|
|
21
|
+
|
|
22
|
+
def propagate_state(self, state: np.array):
|
|
23
|
+
"""
|
|
24
|
+
Propagate the state from k-1 to k
|
|
25
|
+
|
|
26
|
+
>>> Phi = np.eye(2)
|
|
27
|
+
>>> nodes = np.array([[0, 0], [0, 10], [10, 0]])
|
|
28
|
+
>>> model = RangePositioning2D(Phi, nodes)
|
|
29
|
+
>>> state_m = np.array([1, 2])
|
|
30
|
+
>>> model.propagate_state(state_m)
|
|
31
|
+
array([1., 2.])
|
|
32
|
+
"""
|
|
33
|
+
|
|
34
|
+
return np.dot(self._Phi, state)
|
|
35
|
+
|
|
36
|
+
def to_observations(self, state: np.array, compute_jacobian: bool = False) -> ModelObs:
|
|
37
|
+
"""
|
|
38
|
+
Convert the state into observations using a range based 2D positioning model
|
|
39
|
+
|
|
40
|
+
>>> Phi = np.eye(2)
|
|
41
|
+
>>> nodes = np.array([[0, 0], [0, 10], [10, 0]])
|
|
42
|
+
>>> model = RangePositioning2D(Phi, nodes)
|
|
43
|
+
>>> state_m = np.array([1, 2])
|
|
44
|
+
>>> model.to_observations(state_m)
|
|
45
|
+
(array([2.23606798, 8.06225775, 9.21954446]), None)
|
|
46
|
+
|
|
47
|
+
>>> model.to_observations(state_m, compute_jacobian=True)
|
|
48
|
+
(array([2.23606798, 8.06225775, 9.21954446]), array([[ 0.4472136 , 0.89442719],
|
|
49
|
+
[ 0.12403473, -0.99227788],
|
|
50
|
+
[-0.97618706, 0.21693046]]))
|
|
51
|
+
"""
|
|
52
|
+
rho = state - self.nodes
|
|
53
|
+
ranges = np.sqrt(np.sum(np.power(rho, 2), axis=1))
|
|
54
|
+
|
|
55
|
+
H = None
|
|
56
|
+
|
|
57
|
+
if compute_jacobian is True:
|
|
58
|
+
H = rho / ranges[:, np.newaxis]
|
|
59
|
+
|
|
60
|
+
return ranges, H
|
|
61
|
+
|
|
62
|
+
def Phi(self):
|
|
63
|
+
"""
|
|
64
|
+
Get the state transition matrix
|
|
65
|
+
|
|
66
|
+
>>> Phi = np.eye(2)
|
|
67
|
+
>>> nodes = np.array([[0, 0], [0, 10], [10, 0]])
|
|
68
|
+
>>> model = RangePositioning2D(Phi, nodes)
|
|
69
|
+
>>> model.Phi()
|
|
70
|
+
array([[1., 0.],
|
|
71
|
+
[0., 1.]])
|
|
72
|
+
"""
|
|
73
|
+
return self._Phi
|
pygnss/filter/ukf.py
ADDED
|
@@ -0,0 +1,319 @@
|
|
|
1
|
+
import logging
|
|
2
|
+
|
|
3
|
+
import numpy as np
|
|
4
|
+
|
|
5
|
+
from . import StateHandler, Model
|
|
6
|
+
|
|
7
|
+
|
|
8
|
+
class Ukf(object):
|
|
9
|
+
"""
|
|
10
|
+
Class to implement the Unscented Kalman Filter (UKF)
|
|
11
|
+
"""
|
|
12
|
+
|
|
13
|
+
def __init__(self,
|
|
14
|
+
x: np.array,
|
|
15
|
+
P: np.array,
|
|
16
|
+
Q: np.array,
|
|
17
|
+
model: Model,
|
|
18
|
+
state_handler: StateHandler,
|
|
19
|
+
alpha: float = 1.0,
|
|
20
|
+
beta: float = 2.0,
|
|
21
|
+
kappa: float = 0.0,
|
|
22
|
+
logger: logging.Logger = logging):
|
|
23
|
+
"""
|
|
24
|
+
Initialize the Ukf filter object
|
|
25
|
+
|
|
26
|
+
:param x: a-priori state (n)
|
|
27
|
+
:param P: a-priori error covariance (n x n)
|
|
28
|
+
:param Q: Process noise covariance (n x n)
|
|
29
|
+
:param model: Object of type Model that describes the underlying estimation model
|
|
30
|
+
:param
|
|
31
|
+
:param alpha: Primary scaling parameter
|
|
32
|
+
:param beta: Secondary scaling parameter (Gaussian assumption)
|
|
33
|
+
:param kappa: Tertiary scaling parameter
|
|
34
|
+
"""
|
|
35
|
+
|
|
36
|
+
# Store the state, that will be propagated
|
|
37
|
+
self.x = x
|
|
38
|
+
self.P = P
|
|
39
|
+
self.Q = Q
|
|
40
|
+
|
|
41
|
+
self.model = model
|
|
42
|
+
self.state_handler = state_handler
|
|
43
|
+
|
|
44
|
+
self.logger = logger
|
|
45
|
+
|
|
46
|
+
self.L = len(x) # Number of parameters
|
|
47
|
+
|
|
48
|
+
alpha2 = alpha * alpha
|
|
49
|
+
|
|
50
|
+
self.lambd = alpha2 * (self.L + kappa) - self.L
|
|
51
|
+
|
|
52
|
+
# Weights can be computed now, based on the setup input
|
|
53
|
+
n_sigma_points = 2 * self.L + 1
|
|
54
|
+
weight_k = 1.0 / (2.0 * (self.L + self.lambd))
|
|
55
|
+
self.w_m = np.ones((n_sigma_points,)) * weight_k
|
|
56
|
+
self.w_c = self.w_m.copy()
|
|
57
|
+
|
|
58
|
+
k = self.lambd/(self.lambd + self.L)
|
|
59
|
+
|
|
60
|
+
self.w_m[0] = k
|
|
61
|
+
self.w_c[0] = k + 1 - alpha2 + beta
|
|
62
|
+
|
|
63
|
+
def process(self, y_k: np.array, R: np.array):
|
|
64
|
+
"""
|
|
65
|
+
Process an observation batch
|
|
66
|
+
|
|
67
|
+
:param y_k: object that contains the observations
|
|
68
|
+
:param R: matrix with the covariance of the measurement (i.e. measurement noise)
|
|
69
|
+
"""
|
|
70
|
+
|
|
71
|
+
# Time update ----------------------------------------------------------
|
|
72
|
+
|
|
73
|
+
chi_p = self._generate_sigma_points()
|
|
74
|
+
|
|
75
|
+
# Obtain the propagated sigma points (chi_m, $\mathcal{X}_{k|k-1}^x$)
|
|
76
|
+
chi_m = np.array([self.model.propagate_state(sigma_point) for sigma_point in chi_p])
|
|
77
|
+
|
|
78
|
+
# From the propagated sigma points, obtain the averaged state ($\hat x_k^-$)
|
|
79
|
+
x_m = np.sum(chi_m * self.w_m[:, np.newaxis], axis=0)
|
|
80
|
+
|
|
81
|
+
# Compute the spread of the sigma points relative to the average
|
|
82
|
+
spread_chi_m = chi_m - x_m
|
|
83
|
+
|
|
84
|
+
# Covariance of the averaged propagated state ($\bf P_k^-$)
|
|
85
|
+
P_m = self.Q + _weighted_average_of_outer_product(spread_chi_m, spread_chi_m, self.w_c)
|
|
86
|
+
|
|
87
|
+
# Propagate the sigma points to the observation space (psi_m, $\mathcal{Y}_{k|k-1}$)
|
|
88
|
+
psi_m = np.array([self.model.to_observations(sigma_point).y_m for sigma_point in chi_m])
|
|
89
|
+
n_dim = len(psi_m.shape)
|
|
90
|
+
if n_dim == 1:
|
|
91
|
+
raise ValueError(f'Unexpected size for sigma point propagation, got [ {n_dim} ], '
|
|
92
|
+
'expected >= 2. Check that the method model.to_observations returns '
|
|
93
|
+
'an array of observations')
|
|
94
|
+
|
|
95
|
+
# Compute the average observation from the given sigma points
|
|
96
|
+
y_m = np.sum(psi_m * self.w_m[:, np.newaxis], axis=0)
|
|
97
|
+
|
|
98
|
+
# Measurement update ---------------------------------------------------
|
|
99
|
+
spread_psi_m = psi_m - y_m
|
|
100
|
+
|
|
101
|
+
P_yy = R + _weighted_average_of_outer_product(spread_psi_m, spread_psi_m, self.w_c)
|
|
102
|
+
P_xy = _weighted_average_of_outer_product(spread_chi_m, spread_psi_m, self.w_c)
|
|
103
|
+
|
|
104
|
+
# Compute state
|
|
105
|
+
self.x = x_m
|
|
106
|
+
self.P = P_m
|
|
107
|
+
|
|
108
|
+
try:
|
|
109
|
+
# Kalman gain ($\mathcal{K}$))
|
|
110
|
+
K = P_xy @ np.linalg.inv(P_yy)
|
|
111
|
+
|
|
112
|
+
self.x = self.x + K @ (y_k - y_m)
|
|
113
|
+
self.P = self.P - K @ P_yy @ K.T
|
|
114
|
+
|
|
115
|
+
except np.linalg.LinAlgError as e:
|
|
116
|
+
self.logger.warning(f'Unable to compute state, keeping previous one. Error: {e}')
|
|
117
|
+
|
|
118
|
+
self.state_handler.process_state(self.x, self.P)
|
|
119
|
+
|
|
120
|
+
def _generate_sigma_points(self) -> np.array:
|
|
121
|
+
"""
|
|
122
|
+
Generate the sigma points
|
|
123
|
+
|
|
124
|
+
>>> x0 = np.array([0.2, 0.6])
|
|
125
|
+
>>> P0 = np.diag([0.8, 0.3])
|
|
126
|
+
>>> ukf_filter = Ukf(x0, P0, None, None, None)
|
|
127
|
+
>>> ukf_filter._generate_sigma_points()
|
|
128
|
+
array([[ 0.2 , 0.6 ],
|
|
129
|
+
[ 1.46491106, 0.6 ],
|
|
130
|
+
[ 0.2 , 1.37459667],
|
|
131
|
+
[-1.06491106, 0.6 ],
|
|
132
|
+
[ 0.2 , -0.17459667]])
|
|
133
|
+
"""
|
|
134
|
+
|
|
135
|
+
# self.P = _make_positive_definite(self.P)
|
|
136
|
+
|
|
137
|
+
sqrt_P = np.linalg.cholesky(self.P)
|
|
138
|
+
|
|
139
|
+
offsets = np.sqrt(self.L + self.lambd) * sqrt_P
|
|
140
|
+
|
|
141
|
+
chi_p = np.vstack([self.x, self.x + offsets.T, self.x - offsets.T])
|
|
142
|
+
|
|
143
|
+
return chi_p
|
|
144
|
+
|
|
145
|
+
|
|
146
|
+
class SquareRootUkf(object):
|
|
147
|
+
"""
|
|
148
|
+
Class to implement the Unscented Kalman Filter (UKF)
|
|
149
|
+
"""
|
|
150
|
+
|
|
151
|
+
def __init__(self,
|
|
152
|
+
x: np.array,
|
|
153
|
+
P: np.array,
|
|
154
|
+
model: Model,
|
|
155
|
+
state_handler: StateHandler,
|
|
156
|
+
alpha: float = 1.0,
|
|
157
|
+
beta: float = 2.0,
|
|
158
|
+
kappa: float = 0.0,
|
|
159
|
+
logger: logging.Logger = logging):
|
|
160
|
+
"""
|
|
161
|
+
Initialize the Ukf filter object
|
|
162
|
+
|
|
163
|
+
:param x: a-priori state
|
|
164
|
+
:param P: a-priori error covariance
|
|
165
|
+
:param Phi: State transition matrix (for the time update)
|
|
166
|
+
:param alpha: Primary scaling parameter
|
|
167
|
+
:param beta: Secondary scaling parameter (Gaussian assumption)
|
|
168
|
+
:param kappa: Tertiary scaling parameter
|
|
169
|
+
"""
|
|
170
|
+
|
|
171
|
+
# Store the state, that will be propagated
|
|
172
|
+
self.x = x
|
|
173
|
+
self.S = np.linalg.cholesky(P)
|
|
174
|
+
|
|
175
|
+
self.model = model
|
|
176
|
+
self.state_handler = state_handler
|
|
177
|
+
|
|
178
|
+
self.logger = logger
|
|
179
|
+
|
|
180
|
+
self.L = len(x) # Number of parameters
|
|
181
|
+
|
|
182
|
+
alpha2 = alpha * alpha
|
|
183
|
+
|
|
184
|
+
self.lambd = alpha2 * (self.L + kappa) - self.L
|
|
185
|
+
|
|
186
|
+
# Weights can be computed now, based on the setup input
|
|
187
|
+
n_sigma_points = 2 * self.L + 1
|
|
188
|
+
weight_k = 1.0 / (2.0 * (self.L + self.lambd))
|
|
189
|
+
self.w_m = np.ones((n_sigma_points,)) * weight_k
|
|
190
|
+
self.w_c = self.w_m.copy()
|
|
191
|
+
|
|
192
|
+
k = self.lambd/(self.lambd + self.L)
|
|
193
|
+
|
|
194
|
+
self.w_m[0] = k
|
|
195
|
+
self.w_c[0] = k + 1 - alpha2 + beta
|
|
196
|
+
|
|
197
|
+
def process(self, y_k):
|
|
198
|
+
"""
|
|
199
|
+
Process an observation batch
|
|
200
|
+
|
|
201
|
+
:param y_k: object that contains the observations
|
|
202
|
+
"""
|
|
203
|
+
|
|
204
|
+
# Time update ----------------------------------------------------------
|
|
205
|
+
|
|
206
|
+
chi_p = self._generate_sigma_points()
|
|
207
|
+
|
|
208
|
+
# Obtain the propagated sigma points (chi_m, $\mathcal{X}_{k|k-1}^x$)
|
|
209
|
+
chi_m = np.array([self.model.propagate_state(sigma_point) for sigma_point in chi_p])
|
|
210
|
+
|
|
211
|
+
# From the propagated sigma points, obtain the averaged state ($\hat x_k^-$)
|
|
212
|
+
x_m = np.sum(chi_m * self.w_m[:, np.newaxis], axis=0)
|
|
213
|
+
|
|
214
|
+
# Compute the spread of the sigma points relative to the average
|
|
215
|
+
spread_chi_m = chi_m - x_m
|
|
216
|
+
|
|
217
|
+
# Covariance of the averaged propagated state ($\bf P_k^-$)
|
|
218
|
+
P_m = _weighted_average_of_outer_product(spread_chi_m, spread_chi_m, self.w_c)
|
|
219
|
+
|
|
220
|
+
# Propagate the sigma points to the observation space (psi_m, $\mathcal{Y}_{k|k-1}$)
|
|
221
|
+
psi_m = np.array([self.model.to_observations(sigma_point) for sigma_point in chi_p])
|
|
222
|
+
|
|
223
|
+
# Compute the average observation from the given sigma points
|
|
224
|
+
y_m = np.sum(psi_m * self.w_m[:, np.newaxis], axis=0)
|
|
225
|
+
|
|
226
|
+
# Measurement update ---------------------------------------------------
|
|
227
|
+
spread_psi_m = psi_m - y_m
|
|
228
|
+
|
|
229
|
+
P_yy = _weighted_average_of_outer_product(spread_psi_m, spread_psi_m, self.w_c)
|
|
230
|
+
P_xy = _weighted_average_of_outer_product(spread_chi_m, spread_psi_m, self.w_c)
|
|
231
|
+
|
|
232
|
+
# Compute state
|
|
233
|
+
self.x = x_m
|
|
234
|
+
self.P = P_m
|
|
235
|
+
|
|
236
|
+
try:
|
|
237
|
+
# Kalman gain ($\mathcal{K}$))
|
|
238
|
+
K = P_xy @ np.linalg.inv(P_yy)
|
|
239
|
+
|
|
240
|
+
self.x = self.x + K @ (y_k - y_m)
|
|
241
|
+
self.P = self.P - K @ P_yy @ K.T
|
|
242
|
+
|
|
243
|
+
# # Ensure positive definite matrix, known in issue in standard UKF
|
|
244
|
+
# # https://stackoverflow.com/questions/67360472/negative-covariance-matrix-in-unscented-kalman-filter
|
|
245
|
+
# # Get the diagonal of the matrix
|
|
246
|
+
# diagonal = np.diag(self.P).copy()
|
|
247
|
+
# diagonal[diagonal < 0] = 0
|
|
248
|
+
# diagonal += 1.0e-5 # small jitter for regularization
|
|
249
|
+
# np.fill_diagonal(self.P, diagonal)
|
|
250
|
+
|
|
251
|
+
except np.linalg.LinAlgError as e:
|
|
252
|
+
self.logger.warning(f'Unable to compute state, keeping previous one. Error: {e}')
|
|
253
|
+
|
|
254
|
+
self.state_handler.process_state(self.x, self.P)
|
|
255
|
+
|
|
256
|
+
def _generate_sigma_points(self) -> np.array:
|
|
257
|
+
|
|
258
|
+
|
|
259
|
+
sqrt_P = np.linalg.cholesky(self.P)
|
|
260
|
+
|
|
261
|
+
chi_p = np.vstack([self.x, self.x + sqrt_P, self.x - sqrt_P])
|
|
262
|
+
|
|
263
|
+
return chi_p
|
|
264
|
+
|
|
265
|
+
|
|
266
|
+
def _weighted_average_of_outer_product(a: np.array, b: np.array, weights: np.array) -> np.array:
|
|
267
|
+
"""
|
|
268
|
+
Computes the weighted average of the outer products of two arrays of lists
|
|
269
|
+
|
|
270
|
+
Given two arrays $a$ and $b$, this method implements
|
|
271
|
+
|
|
272
|
+
$$
|
|
273
|
+
P = \\sum_{i=0}^N w_i \\cdot \\left( a_i \\cdot b_i^T \\right)
|
|
274
|
+
$$
|
|
275
|
+
|
|
276
|
+
|
|
277
|
+
>>> a = np.array([[1, 2], [3, 4]])
|
|
278
|
+
>>> b = np.array([[5, 6], [7, 8]])
|
|
279
|
+
>>> weights = np.array([1, 2])
|
|
280
|
+
>>> _weighted_average_of_outer_product(a, b, weights)
|
|
281
|
+
array([[47, 54],
|
|
282
|
+
[66, 76]])
|
|
283
|
+
"""
|
|
284
|
+
|
|
285
|
+
if a.shape[0] != b.shape[0]:
|
|
286
|
+
raise ValueError(f'Number of rows in input vectors differ: [ {a.shape[0]} ] != [ {b.shape[0]} ]')
|
|
287
|
+
|
|
288
|
+
elif a.shape[0] != len(weights):
|
|
289
|
+
raise ValueError(f'Incorrect size of the weights vector: [ {a.shape[0]} ] != [ {len(weights)} ]')
|
|
290
|
+
|
|
291
|
+
n_rows = a.shape[0]
|
|
292
|
+
|
|
293
|
+
products = [np.outer(a[i], b[i]) * weights[i] for i in range(n_rows)]
|
|
294
|
+
|
|
295
|
+
average = np.sum(products, axis=0)
|
|
296
|
+
|
|
297
|
+
return average
|
|
298
|
+
|
|
299
|
+
|
|
300
|
+
def _make_positive_definite(matrix, epsilon=1e-6):
|
|
301
|
+
"""
|
|
302
|
+
Makes a matrix positive definite by adding a small value to its diagonal.
|
|
303
|
+
|
|
304
|
+
Args:
|
|
305
|
+
matrix: The input matrix (NumPy array).
|
|
306
|
+
epsilon: A small positive value to add to the diagonal (default: 1e-6).
|
|
307
|
+
|
|
308
|
+
Returns:
|
|
309
|
+
A positive definite matrix.
|
|
310
|
+
"""
|
|
311
|
+
|
|
312
|
+
eigenvalues, _ = np.linalg.eig(matrix)
|
|
313
|
+
min_eigenvalue = np.min(eigenvalues)
|
|
314
|
+
|
|
315
|
+
if min_eigenvalue < 0:
|
|
316
|
+
shift = -min_eigenvalue + epsilon
|
|
317
|
+
return matrix + shift * np.eye(matrix.shape[0])
|
|
318
|
+
else:
|
|
319
|
+
return matrix
|
pygnss/hatanaka.py
CHANGED
|
@@ -4,10 +4,15 @@ import tempfile
|
|
|
4
4
|
|
|
5
5
|
from pygnss._c_ext import _read_crx
|
|
6
6
|
|
|
7
|
-
def to_dataframe(filename:str, station:str = "none") -> pd.DataFrame:
|
|
7
|
+
def to_dataframe(filename:str, station:str = "none", strict_lli: bool = True) -> pd.DataFrame:
|
|
8
8
|
"""
|
|
9
9
|
Convert a Compressed (crx.gz) or uncompressed (crx) Hatanaka file into a
|
|
10
10
|
DataFrame
|
|
11
|
+
|
|
12
|
+
:param filename: Hatanaka [gzip compressed] filename
|
|
13
|
+
:param station: force station name
|
|
14
|
+
:param strict_lli: Mark cycle slips only when Phase LLI is 1 (as per RINEX convention).
|
|
15
|
+
If False, any value of Phase LLI will trigger a cycle slip flag
|
|
11
16
|
"""
|
|
12
17
|
|
|
13
18
|
if filename.endswith('crx.gz') or filename.endswith('crx.Z') or filename.endswith('crz'):
|
|
@@ -23,13 +28,24 @@ def to_dataframe(filename:str, station:str = "none") -> pd.DataFrame:
|
|
|
23
28
|
else:
|
|
24
29
|
array = _read_crx(filename)
|
|
25
30
|
|
|
26
|
-
df = pd.DataFrame(array, columns=['epoch', 'sat', 'rinex3_code', 'value'])
|
|
31
|
+
df = pd.DataFrame(array, columns=['epoch', 'sat', 'rinex3_code', 'value', 'lli'])
|
|
27
32
|
df['channel'] = df['rinex3_code'].str[-2:]
|
|
28
33
|
df['signal'] = df['sat'] + df['channel']
|
|
29
34
|
MAPPING = {'C': 'range', 'L': 'phase', 'D': 'doppler', 'S': 'snr'}
|
|
30
35
|
df['obstype'] = df['rinex3_code'].str[0].map(lambda x: MAPPING.get(x, 'Unknown'))
|
|
31
|
-
df = df.pivot_table(index=['epoch', 'signal', 'sat', 'channel'], columns=['obstype'], values='value')
|
|
36
|
+
df = df.pivot_table(index=['epoch', 'signal', 'sat', 'channel'], columns=['obstype'], values=['value', 'lli'])
|
|
37
|
+
|
|
38
|
+
# Remove all LLI columns except for the phase (for the cycle slips)
|
|
39
|
+
if strict_lli:
|
|
40
|
+
df['cslip'] = (df.loc[:, pd.IndexSlice['lli', 'phase']] % 2) == 1
|
|
41
|
+
else:
|
|
42
|
+
df['cslip'] = df.loc[:, pd.IndexSlice['lli', 'phase']] > 0
|
|
43
|
+
|
|
44
|
+
df.drop('lli', axis=1, inplace=True)
|
|
45
|
+
df.columns = [v[1] if v[0] == 'value' else v[0] for v in df.columns.values]
|
|
46
|
+
|
|
32
47
|
df.reset_index(inplace=True)
|
|
48
|
+
|
|
33
49
|
df['station'] = station
|
|
34
50
|
|
|
35
51
|
return df
|
|
@@ -1,6 +1,6 @@
|
|
|
1
1
|
Metadata-Version: 2.2
|
|
2
2
|
Name: pygnss
|
|
3
|
-
Version: 0.
|
|
3
|
+
Version: 0.1.1
|
|
4
4
|
Summary: Package with utilities and tools for GNSS data processing
|
|
5
5
|
Author-email: Miquel Garcia-Fernandez <miquel@mgfernan.com>
|
|
6
6
|
License: MIT
|
|
@@ -18,19 +18,13 @@ Requires-Dist: flake8>=7.0.0; extra == "test"
|
|
|
18
18
|
Provides-Extra: release
|
|
19
19
|
Requires-Dist: python-semantic-release>=9.4.0; extra == "release"
|
|
20
20
|
|
|
21
|
-
#
|
|
22
|
-
|
|
23
|
-
Python tools used in internal Rokubun projects. This repository contains the following modules:
|
|
24
|
-
|
|
25
|
-
- `logger`, a module that extends basic Python logging
|
|
26
|
-
- `geodetic`, to perform basic geodetic transformation (Cartesian to Geodetic,
|
|
27
|
-
Cartesian to Local Tangential Plane, ...)
|
|
21
|
+
# GNSS and Navigation modules
|
|
28
22
|
|
|
29
23
|
## Installation
|
|
30
24
|
|
|
31
25
|
To make sure that the extensions are installed along with the package, run
|
|
32
26
|
|
|
33
|
-
`pip install pygnss
|
|
27
|
+
`pip install pygnss`
|
|
34
28
|
|
|
35
29
|
## Modules
|
|
36
30
|
|
|
@@ -58,10 +52,3 @@ Traceback (most recent call last):
|
|
|
58
52
|
...
|
|
59
53
|
ValueError: Exception message
|
|
60
54
|
```
|
|
61
|
-
|
|
62
|
-
## Deployment to PyPi
|
|
63
|
-
|
|
64
|
-
The project is published automatically using internal Gitlab CI on each commit to `trunk` to PyPi repository [pygnss](https://pypi.org/project/pygnss/)
|
|
65
|
-
|
|
66
|
-
It uses semantic versioning and conventional commits to set the version and [semantic-release](https://python-semantic-release.readthedocs.io/en/latest/index.html) as
|
|
67
|
-
versioning tool.
|
|
@@ -1,4 +1,4 @@
|
|
|
1
|
-
pygnss/hatanaka.py,sha256=
|
|
1
|
+
pygnss/hatanaka.py,sha256=P9XG6bZwUzfAPYn--6-DXfFQIEefeimE7fMJm_DF5zE,1951
|
|
2
2
|
pygnss/file.py,sha256=kkMBWjoTPkxJD1UgH0mXJT2fxnhU8u7_l2Ph5Xz2-hY,933
|
|
3
3
|
pygnss/rinex.py,sha256=LsOOh3Fc263kkM8KOUBNeMeIAmbOn2ASSBO4rAUJWj8,68783
|
|
4
4
|
pygnss/sinex.py,sha256=nErOmGCFFmGSnmWGNTJhaj3yZ6IIB8GgtW5WPypJc6U,3057
|
|
@@ -9,11 +9,15 @@ pygnss/time.py,sha256=YdMNs2xA43LrSgEOgB7jpEq0dCWv89fUBF5syDLjbu0,11178
|
|
|
9
9
|
pygnss/logger.py,sha256=4kvcTWXPoiG-MlyP6B330l4Fu7MfCuDjuIlIiLA8f1Y,1479
|
|
10
10
|
pygnss/decorator.py,sha256=ldlZuvwuIlJf2pkoWteyXyp5tLds8KRkphrPsrURw9U,491
|
|
11
11
|
pygnss/stats.py,sha256=mDiY0K-VTndlFEkbxTzq9PYxCOjYDYsY3ZQV0PuMREM,1924
|
|
12
|
-
pygnss/_c_ext.cpython-312-i386-linux-musl.so,sha256=
|
|
13
|
-
pygnss/__init__.py,sha256=
|
|
12
|
+
pygnss/_c_ext.cpython-312-i386-linux-musl.so,sha256=DqlO94ztgRQBT7f9-c1z-Fh79DkP1HnthltlRRrm4zQ,80580
|
|
13
|
+
pygnss/__init__.py,sha256=rnObPjuBcEStqSO0S6gsdS_ot8ITOQjVj_-P1LUUYpg,22
|
|
14
14
|
pygnss/constants.py,sha256=1hF6K92X6E6Ofo0rAuCBCgrwln9jxio26RV2a6vyURk,133
|
|
15
|
+
pygnss/filter/models.py,sha256=gXq7-YBcAoDq4-7Wr0ChNWxwXr9m1EEhUnlLtKVlsAQ,2165
|
|
16
|
+
pygnss/filter/__init__.py,sha256=Ek5NM48EiDbnjYDz7l1QLojkAQre5tzPjCgssH0hwoU,1830
|
|
17
|
+
pygnss/filter/ekf.py,sha256=wtjjXbeJ7_MSL32dMsoTcppEAaWvqMNuDIcMmDCwyFQ,1871
|
|
18
|
+
pygnss/filter/ukf.py,sha256=wEgDKV6VpEIIZl2KG3sLT0HA-K8yAw9UI0WlA1gyYm0,10500
|
|
15
19
|
pygnss/_c_ext/src/mtable_init.c,sha256=5w869E6PX-ca9UHhKBxLFRW694-VaNwGlMs0I5v99mk,1132
|
|
16
|
-
pygnss/_c_ext/src/hatanaka.c,sha256=
|
|
20
|
+
pygnss/_c_ext/src/hatanaka.c,sha256=YNWaMzQQQnTNls5J6TMNuyhlq505NGDfzU-MJAHab8Q,2520
|
|
17
21
|
pygnss/_c_ext/src/helpers.c,sha256=gINr73ktRgox_S7fYdFR58lLqAUACRpJfog4M5BW1-Q,364
|
|
18
22
|
pygnss/parsers/rtklib/stats.py,sha256=YV6yadxMeQMQYZvsUCaSf4ZTpK8Bbv3f2xgu0l4PekA,5449
|
|
19
23
|
pygnss/orbit/kepler.py,sha256=QORTgg5yBtsQXxLWSzoZ1pmh-CwPiZlFdIYqhQhv1a0,1745
|
|
@@ -24,9 +28,9 @@ pygnss/gnss/residuals.py,sha256=8qKGNOYkrqxHGOSjIfH21K82PAqEh2068kf78j5usL8,1244
|
|
|
24
28
|
pygnss/gnss/edit.py,sha256=T1r0WbJmt8tLJpG_IIsy4Atej6cy0IStBaSGxw0S5ho,1884
|
|
25
29
|
pygnss/gnss/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
26
30
|
pygnss/gnss/observables.py,sha256=0x0NLkTjxf8cO9F_f_Q1b-1hEeoNjWB2x-53ecUEv0M,1656
|
|
27
|
-
pygnss-0.
|
|
28
|
-
pygnss-0.
|
|
29
|
-
pygnss-0.
|
|
30
|
-
pygnss-0.
|
|
31
|
-
pygnss-0.
|
|
32
|
-
pygnss-0.
|
|
31
|
+
pygnss-0.1.1.dist-info/LICENSE,sha256=Wwany6RAAZ9vVHjFLA9KBJ0HE77d52s2NOUA1CPAEug,1067
|
|
32
|
+
pygnss-0.1.1.dist-info/WHEEL,sha256=5xfeZaWcUxmahjMEV-z0DMG7R8tonf6LlNO0IMSCOqM,110
|
|
33
|
+
pygnss-0.1.1.dist-info/RECORD,,
|
|
34
|
+
pygnss-0.1.1.dist-info/top_level.txt,sha256=oZRSR-qOv98VW2PRRMGCVNCJmewcJjyJYmxzxfeimtg,7
|
|
35
|
+
pygnss-0.1.1.dist-info/entry_points.txt,sha256=mCuKrljB_wh9ZQVROiId9m68EDbTiY1oef_L1N3IDDA,262
|
|
36
|
+
pygnss-0.1.1.dist-info/METADATA,sha256=hli0z6E8sLmwdA3YOFCR1g6QoQcZMfr6PCsEnLYF_Kc,1614
|
|
File without changes
|
|
File without changes
|
|
File without changes
|
|
File without changes
|