Skip to content

Tracking

sies.tracking.kalman

Extended Kalman Filter and target motion model.

The state vector is [vx, vy, x, y, phi]: velocity, position and angular position of the target.

ExtendedKalmanFilter 

ExtendedKalmanFilter(state_matrix, process_cov, observation, observation_jacobian, observation_cov)

Extended Kalman Filter with a linear state equation.

Parameters:

Name Type Description Default
state_matrix (ndarray, shape(d, d))

State transition matrix F.

 required
process_cov (ndarray, shape(d, d))

Process noise covariance Q.

 required
observation callable

Observation function h(state) -> ndarray.

 required
observation_jacobian callable

Jacobian dh(state) -> ndarray of the observation function.

 required
observation_cov ndarray

Observation noise covariance R.

 required
Source code in src/sies/tracking/kalman.py 
127
128
129
130
131
132
133
134
135
136
137
138
139
def __init__(
    self,
    state_matrix: NDArray,
    process_cov: NDArray,
    observation: Callable[[NDArray], NDArray],
    observation_jacobian: Callable[[NDArray], NDArray],
    observation_cov: NDArray,
):
    self.state_matrix = state_matrix
    self.process_cov = process_cov
    self.observation = observation
    self.observation_jacobian = observation_jacobian
    self.observation_cov = observation_cov

run 

run(measurements, initial_state, initial_cov=None)

Filter a stream of measurements.

The filter follows the standard predict-update cycle: at each step the previous estimate is propagated through the state equation before being corrected by the measurement. The initial_state is therefore the estimate prior to the first measurement (at time step -1).

Parameters:

Name Type Description Default
measurements (ndarray, shape(m, nb_steps))

One measurement vector per time step.

 required
initial_state (ndarray, shape(d))

State estimate one step before the first measurement.

 required
initial_cov (ndarray, shape(d, d))

Covariance of the initial state estimate; zero by default (the initial state is trusted exactly).

 None

Returns:

Type Description
(ndarray, shape(d, nb_steps))

The estimated state at each time step.
Source code in src/sies/tracking/kalman.py 
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
def run(
    self,
    measurements: NDArray,
    initial_state: NDArray,
    initial_cov: NDArray | None = None,
) -> NDArray:
    """Filter a stream of measurements.

    The filter follows the standard predict-update cycle: at each
    step the previous estimate is propagated through the state
    equation before being corrected by the measurement. The
    `initial_state` is therefore the estimate *prior* to the first
    measurement (at time step -1).

    Parameters
    ----------
    measurements : ndarray, shape (m, nb_steps)
        One measurement vector per time step.
    initial_state : ndarray, shape (d,)
        State estimate one step before the first measurement.
    initial_cov : ndarray, shape (d, d), optional
        Covariance of the initial state estimate; zero by default
        (the initial state is trusted exactly).

    Returns
    -------
    ndarray, shape (d, nb_steps)
        The estimated state at each time step.
    """
    nb_steps = measurements.shape[1]
    state = np.asarray(initial_state, dtype=float).reshape(-1)
    cov = np.zeros_like(self.process_cov) if initial_cov is None else initial_cov

    estimates = np.zeros((state.size, nb_steps))
    for n in range(nb_steps):
        # Prediction
        predicted = self.state_matrix @ state
        predicted_cov = self.state_matrix @ cov @ self.state_matrix.T + self.process_cov

        # Update
        innovation = measurements[:, n] - self.observation(predicted)
        jacobian = self.observation_jacobian(predicted)
        innovation_cov = jacobian @ predicted_cov @ jacobian.T + self.observation_cov
        # gain = P H^T S^-1, computed by solving the (symmetric)
        # system instead of forming the explicit inverse.
        gain = np.linalg.solve(innovation_cov, jacobian @ predicted_cov).T

        state = predicted + gain @ innovation
        cov = predicted_cov - gain @ jacobian @ predicted_cov
        estimates[:, n] = state

    return estimates

target_dynamics 

target_dynamics(dt, std_acc, std_acc_angle)

Build the matrices of the linear target motion model.

The target follows a constant-velocity model driven by white acceleration noise.

Parameters:

Name Type Description Default
dt float

Time step.

 required
std_acc float

Standard deviation of the linear acceleration noise.

 required
std_acc_angle float

Standard deviation of the angular acceleration noise.

 required

Returns:

Name Type Description
state_matrix (ndarray, shape(5, 5))

State transition matrix F.
noise_cov (ndarray, shape(5, 5))

Process noise covariance Q.
noise_gain (ndarray, shape(5, 5))

Gain Q0 mapping accelerations to state increments (used by the simulator).
Source code in src/sies/tracking/kalman.py 
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
def target_dynamics(
    dt: float, std_acc: float, std_acc_angle: float
) -> tuple[NDArray, NDArray, NDArray]:
    """Build the matrices of the linear target motion model.

    The target follows a constant-velocity model driven by white
    acceleration noise.

    Parameters
    ----------
    dt : float
        Time step.
    std_acc : float
        Standard deviation of the linear acceleration noise.
    std_acc_angle : float
        Standard deviation of the angular acceleration noise.

    Returns
    -------
    state_matrix : ndarray, shape (5, 5)
        State transition matrix ``F``.
    noise_cov : ndarray, shape (5, 5)
        Process noise covariance ``Q``.
    noise_gain : ndarray, shape (5, 5)
        Gain ``Q0`` mapping accelerations to state increments (used by
        the simulator).
    """
    state_matrix = np.eye(5)
    state_matrix[2, 0] = dt
    state_matrix[3, 1] = dt

    noise_gain = np.diag([dt, dt, dt**2 / 2, dt**2 / 2, dt])

    acc_block = std_acc**2 * np.eye(2)
    sigma = np.zeros((5, 5))
    sigma[:2, :2] = acc_block
    sigma[:2, 2:4] = acc_block
    sigma[2:4, :2] = acc_block
    sigma[2:4, 2:4] = acc_block
    sigma[4, 4] = std_acc_angle**2
    noise_cov = noise_gain @ sigma @ noise_gain.T

    return state_matrix, noise_cov, noise_gain

simulate_target_path 

simulate_target_path(dt, nb_steps, initial_state, std_acc, std_acc_angle, rng=None)

Simulate a random target trajectory.

Parameters:

Name Type Description Default
dt float

Time step.

 required
nb_steps int

Number of time steps.

 required
initial_state (ndarray, shape(5))

Initial state [vx, vy, x, y, phi].

 required
std_acc float

Standard deviation of the linear acceleration noise.

 required
std_acc_angle float

Standard deviation of the angular acceleration noise.

 required
rng Generator

Random generator, for reproducibility.

 None

Returns:

Type Description
(ndarray, shape(5, nb_steps))

The simulated state at each time step.

Raises:

Type Description
ValueError

If nb_steps is not at least one.
Source code in src/sies/tracking/kalman.py 
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
def simulate_target_path(
    dt: float,
    nb_steps: int,
    initial_state: NDArray,
    std_acc: float,
    std_acc_angle: float,
    rng: np.random.Generator | None = None,
) -> NDArray:
    """Simulate a random target trajectory.

    Parameters
    ----------
    dt : float
        Time step.
    nb_steps : int
        Number of time steps.
    initial_state : ndarray, shape (5,)
        Initial state ``[vx, vy, x, y, phi]``.
    std_acc : float
        Standard deviation of the linear acceleration noise.
    std_acc_angle : float
        Standard deviation of the angular acceleration noise.
    rng : numpy.random.Generator, optional
        Random generator, for reproducibility.

    Returns
    -------
    ndarray, shape (5, nb_steps)
        The simulated state at each time step.

    Raises
    ------
    ValueError
        If `nb_steps` is not at least one.
    """
    if nb_steps < 1:
        raise ValueError("`nb_steps` must be >= 1.")
    rng = rng or np.random.default_rng()
    state_matrix, _, noise_gain = target_dynamics(dt, std_acc, std_acc_angle)

    path = np.zeros((5, nb_steps))
    path[:, 0] = np.asarray(initial_state, dtype=float).reshape(5)
    for n in range(1, nb_steps):
        acc = std_acc * rng.standard_normal(2)
        acc_angle = std_acc_angle * rng.standard_normal()
        noise = noise_gain @ np.concatenate([acc, acc, [acc_angle]])
        path[:, n] = state_matrix @ path[:, n - 1] + noise
    return path

sies.tracking.observation

Observation model of the tracking problem.

The observation is the MSR matrix produced by a target of known CGPT located at the (unknown) position and orientation encoded in the state vector [vx, vy, x, y, phi].

CGPTObservation 

CGPTObservation(src_matrix, rcv_matrix, cgpt)

MSR observation of a moving target with known CGPT.

The forward model is MSR = As M(x, phi) Ar^T where M(x, phi) is the CGPT of the target translated to x and rotated by phi.

Parameters:

Name Type Description Default
src_matrix ndarray, shape (ns, 2k)

Source-side acquisition matrix As.

 required
rcv_matrix ndarray, shape (nr, 2k)

Receiver-side acquisition matrix Ar.

 required
cgpt ndarray, shape (2k, 2k)

CGPT matrix of the target at the origin with orientation zero.

 required
Source code in src/sies/tracking/observation.py 
34
35
36
37
38
39
40
41
42
43
44
def __init__(self, src_matrix: NDArray, rcv_matrix: NDArray, cgpt: NDArray):
    if cgpt.shape[0] != cgpt.shape[1]:
        raise ValueError("CGPT matrix must be square.")
    if cgpt.shape[0] % 2 != 0:
        raise ValueError("CGPT matrix size must be even (shape (2k, 2k)).")
    if src_matrix.shape[1] != cgpt.shape[0] or rcv_matrix.shape[1] != cgpt.shape[0]:
        raise ValueError("Acquisition matrix columns must match the CGPT size.")
    self.src_matrix = src_matrix
    self.rcv_matrix = rcv_matrix
    self.cgpt = cgpt
    self._order = cgpt.shape[0] // 2

__call__ 

__call__(state)

Evaluate the observation function h.

Parameters:

Name Type Description Default
state (ndarray, shape(5))

State vector [vx, vy, x, y, phi].

 required

Returns:

Type Description
(ndarray, shape(ns * nr))

The vectorized MSR matrix.
Source code in src/sies/tracking/observation.py 
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
def __call__(self, state: NDArray) -> NDArray:
    """Evaluate the observation function ``h``.

    Parameters
    ----------
    state : ndarray, shape (5,)
        State vector ``[vx, vy, x, y, phi]``.

    Returns
    -------
    ndarray, shape (ns * nr,)
        The vectorized MSR matrix.
    """
    translation = state[2] + 1j * state[3]
    moved = transform_cgpt(self.cgpt, translation, 1.0, state[4])
    return (self.src_matrix @ moved @ self.rcv_matrix.T).ravel()

jacobian 

jacobian(state)

Evaluate the Jacobian of the observation function.

The derivatives are taken with respect to the five state variables; only the position and orientation derivatives are nonzero.

Parameters:

Name Type Description Default
state (ndarray, shape(5))

State vector [vx, vy, x, y, phi].

 required

Returns:

Type Description
(ndarray, shape(ns * nr, 5))

The Jacobian matrix.
Source code in src/sies/tracking/observation.py 
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
def jacobian(self, state: NDArray) -> NDArray:
    """Evaluate the Jacobian of the observation function.

    The derivatives are taken with respect to the five state
    variables; only the position and orientation derivatives are
    nonzero.

    Parameters
    ----------
    state : ndarray, shape (5,)
        State vector ``[vx, vy, x, y, phi]``.

    Returns
    -------
    ndarray, shape (ns * nr, 5)
        The Jacobian matrix.
    """
    order = self._order
    t0 = state[2] + 1j * state[3]
    phi = state[4]

    # Transformation in the complex representation: M -> J^T M J with
    # J = U F, U the embedding of C^k in R^(2k).
    embed = np.kron(np.eye(order), np.array([[1.0], [1.0j]]))
    re_u, im_u = embed.real, embed.imag

    m_idx = np.arange(1, order + 1)[:, np.newaxis]
    n_idx = np.arange(1, order + 1)[np.newaxis, :]
    binom = np.triu(comb(n_idx, m_idx))
    rotation = np.exp(1j * m_idx * phi)

    diff = n_idx - m_idx
    powers = (t0 + 0j) ** np.maximum(diff, 0)
    transform = binom * powers * rotation

    d_powers = np.where(diff >= 1, diff * (t0 + 0j) ** np.maximum(diff - 1, 0), 0)
    d_x = binom * d_powers * rotation
    d_y = 1j * d_x
    d_phi = transform * (1j * m_idx)

    jac = np.zeros((self.src_matrix.shape[0] * self.rcv_matrix.shape[0], 5))
    for column, d_transform in ((2, d_x), (3, d_y), (4, d_phi)):
        jac[:, column] = self._assemble_derivative(embed, re_u, im_u, transform, d_transform)
    return jac