kalbee 0.1.0__py3-none-any.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.
- kalbee/__init__.py +17 -0
- kalbee/modules/__init__.py +0 -0
- kalbee/modules/filters/__init__.py +13 -0
- kalbee/modules/filters/abg_filter.py +72 -0
- kalbee/modules/filters/auto_filter.py +46 -0
- kalbee/modules/filters/base.py +106 -0
- kalbee/modules/filters/ekf_filter.py +149 -0
- kalbee/modules/filters/kf_filter.py +75 -0
- kalbee-0.1.0.dist-info/LICENSE +201 -0
- kalbee-0.1.0.dist-info/METADATA +117 -0
- kalbee-0.1.0.dist-info/RECORD +13 -0
- kalbee-0.1.0.dist-info/WHEEL +5 -0
- kalbee-0.1.0.dist-info/top_level.txt +1 -0
kalbee/__init__.py
ADDED
|
@@ -0,0 +1,17 @@
|
|
|
1
|
+
from kalbee.modules.filters import (
|
|
2
|
+
BaseFilter,
|
|
3
|
+
KalmanFilter,
|
|
4
|
+
ExtendedKalmanFilter,
|
|
5
|
+
AlphaBetaGammaFilter,
|
|
6
|
+
AutoFilter,
|
|
7
|
+
)
|
|
8
|
+
|
|
9
|
+
__version__ = "0.1.0"
|
|
10
|
+
|
|
11
|
+
__all__ = [
|
|
12
|
+
"BaseFilter",
|
|
13
|
+
"KalmanFilter",
|
|
14
|
+
"ExtendedKalmanFilter",
|
|
15
|
+
"AlphaBetaGammaFilter",
|
|
16
|
+
"AutoFilter",
|
|
17
|
+
]
|
|
File without changes
|
|
@@ -0,0 +1,13 @@
|
|
|
1
|
+
from kalbee.modules.filters.base import BaseFilter
|
|
2
|
+
from kalbee.modules.filters.kf_filter import KalmanFilter
|
|
3
|
+
from kalbee.modules.filters.abg_filter import AlphaBetaGammaFilter
|
|
4
|
+
from kalbee.modules.filters.ekf_filter import ExtendedKalmanFilter
|
|
5
|
+
from kalbee.modules.filters.auto_filter import AutoFilter
|
|
6
|
+
|
|
7
|
+
__all__ = [
|
|
8
|
+
"BaseFilter",
|
|
9
|
+
"KalmanFilter",
|
|
10
|
+
"AlphaBetaGammaFilter",
|
|
11
|
+
"ExtendedKalmanFilter",
|
|
12
|
+
"AutoFilter",
|
|
13
|
+
]
|
|
@@ -0,0 +1,72 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
from typing import Optional
|
|
3
|
+
from kalbee.modules.filters.base import BaseFilter
|
|
4
|
+
|
|
5
|
+
|
|
6
|
+
class AlphaBetaGammaFilter(BaseFilter):
|
|
7
|
+
"""
|
|
8
|
+
Implementation of the Alpha-Beta-Gamma filter.
|
|
9
|
+
Used for tracking position, velocity, and acceleration with constant gains.
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
def __init__(
|
|
13
|
+
self,
|
|
14
|
+
state: np.ndarray,
|
|
15
|
+
alpha: float,
|
|
16
|
+
beta: float,
|
|
17
|
+
gamma: float,
|
|
18
|
+
initial_covariance: Optional[np.ndarray] = None,
|
|
19
|
+
):
|
|
20
|
+
"""
|
|
21
|
+
Initialize the alpha-beta-gamma filter.
|
|
22
|
+
|
|
23
|
+
Args:
|
|
24
|
+
state: Initial state vector [x, v, a]^T
|
|
25
|
+
alpha: Position gain
|
|
26
|
+
beta: Velocity gain
|
|
27
|
+
gamma: Acceleration gain
|
|
28
|
+
initial_covariance: Optional initial covariance (P)
|
|
29
|
+
"""
|
|
30
|
+
if initial_covariance is None:
|
|
31
|
+
# Default identity covariance if not provided
|
|
32
|
+
initial_covariance = np.eye(len(state))
|
|
33
|
+
|
|
34
|
+
super().__init__(state, initial_covariance)
|
|
35
|
+
self.alpha = alpha
|
|
36
|
+
self.beta = beta
|
|
37
|
+
self.gamma = gamma
|
|
38
|
+
|
|
39
|
+
def predict(self, dt: float = 1.0, **kwargs) -> np.ndarray:
|
|
40
|
+
"""
|
|
41
|
+
Advance the state using the kinematics model:
|
|
42
|
+
x_k = x_{k-1} + v_{k-1}*dt + 0.5*a_{k-1}*dt^2
|
|
43
|
+
v_k = v_{k-1} + a_{k-1}*dt
|
|
44
|
+
a_k = a_{k-1}
|
|
45
|
+
"""
|
|
46
|
+
x, v, a = self.state.flatten()
|
|
47
|
+
|
|
48
|
+
new_x = x + v * dt + 0.5 * a * (dt**2)
|
|
49
|
+
new_v = v + a * dt
|
|
50
|
+
new_a = a
|
|
51
|
+
|
|
52
|
+
self.state = np.array([[new_x], [new_v], [new_a]])
|
|
53
|
+
|
|
54
|
+
# In ABG filters, we typically don't update P during predict
|
|
55
|
+
# as gains are fixed.
|
|
56
|
+
return self.state
|
|
57
|
+
|
|
58
|
+
def update(self, measurement: np.ndarray, dt: float = 1.0, **kwargs) -> np.ndarray:
|
|
59
|
+
"""
|
|
60
|
+
Update the state using the residual and fixed gains alpha, beta, gamma.
|
|
61
|
+
"""
|
|
62
|
+
# Residual (innovation) - comparing measurement to predicted position
|
|
63
|
+
z = np.asanyarray(measurement).flatten()[0]
|
|
64
|
+
x_pred = self.state[0, 0]
|
|
65
|
+
residual = z - x_pred
|
|
66
|
+
|
|
67
|
+
# Apply corrections
|
|
68
|
+
self.state[0, 0] += self.alpha * residual
|
|
69
|
+
self.state[1, 0] += (self.beta / dt) * residual
|
|
70
|
+
self.state[2, 0] += (self.gamma / (2 * dt**2)) * residual
|
|
71
|
+
|
|
72
|
+
return self.state
|
|
@@ -0,0 +1,46 @@
|
|
|
1
|
+
from kalbee.modules.filters.base import BaseFilter
|
|
2
|
+
from kalbee.modules.filters.kf_filter import KalmanFilter
|
|
3
|
+
from kalbee.modules.filters.abg_filter import AlphaBetaGammaFilter
|
|
4
|
+
from kalbee.modules.filters.ekf_filter import ExtendedKalmanFilter
|
|
5
|
+
|
|
6
|
+
|
|
7
|
+
class AutoFilter:
|
|
8
|
+
"""
|
|
9
|
+
Factory class for creating filter instances.
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
@staticmethod
|
|
13
|
+
def from_filter(*args, mode: str = "kalmanfilter", **kwargs) -> BaseFilter:
|
|
14
|
+
"""
|
|
15
|
+
Create a filter instance based on the specified mode.
|
|
16
|
+
|
|
17
|
+
Args:
|
|
18
|
+
mode: The type of filter to create.
|
|
19
|
+
Options:
|
|
20
|
+
- 'kalmanfilter', 'kf', 'kalman'
|
|
21
|
+
- 'extendedkalmanfilter', 'ekf'
|
|
22
|
+
- 'alphabetagamma', 'abg'
|
|
23
|
+
*args: Positional arguments passed to the filter constructor.
|
|
24
|
+
**kwargs: Keyword arguments passed to the filter constructor.
|
|
25
|
+
|
|
26
|
+
Returns:
|
|
27
|
+
An instance of a BaseFilter subclass.
|
|
28
|
+
|
|
29
|
+
Raises:
|
|
30
|
+
ValueError: If the mode is unknown.
|
|
31
|
+
"""
|
|
32
|
+
mode_clean = mode.lower().replace("_", "").replace("-", "")
|
|
33
|
+
|
|
34
|
+
if mode_clean in ["kalmanfilter", "kf", "kalman"]:
|
|
35
|
+
return KalmanFilter(*args, **kwargs)
|
|
36
|
+
|
|
37
|
+
elif mode_clean in ["extendedkalmanfilter", "ekf"]:
|
|
38
|
+
return ExtendedKalmanFilter(*args, **kwargs)
|
|
39
|
+
|
|
40
|
+
elif mode_clean in ["alphabetagamma", "abg"]:
|
|
41
|
+
return AlphaBetaGammaFilter(*args, **kwargs)
|
|
42
|
+
|
|
43
|
+
else:
|
|
44
|
+
raise ValueError(
|
|
45
|
+
f"Unknown filter mode: '{mode}'. Available modes: kalmanfilter, ekf, abg."
|
|
46
|
+
)
|
|
@@ -0,0 +1,106 @@
|
|
|
1
|
+
from abc import ABC, abstractmethod
|
|
2
|
+
from typing import Optional
|
|
3
|
+
import numpy as np
|
|
4
|
+
|
|
5
|
+
|
|
6
|
+
class BaseFilter(ABC):
|
|
7
|
+
"""
|
|
8
|
+
Abstract base class for state estimation filters.
|
|
9
|
+
|
|
10
|
+
This class defines the interface for filters like Kalman Filter,
|
|
11
|
+
Extended Kalman Filter, and alpha-beta-gamma filters.
|
|
12
|
+
"""
|
|
13
|
+
|
|
14
|
+
def __init__(
|
|
15
|
+
self,
|
|
16
|
+
state: np.ndarray,
|
|
17
|
+
covariance: np.ndarray,
|
|
18
|
+
transition_matrix: Optional[np.ndarray] = None,
|
|
19
|
+
transition_covariance: Optional[np.ndarray] = None,
|
|
20
|
+
measurement_matrix: Optional[np.ndarray] = None,
|
|
21
|
+
measurement_covariance: Optional[np.ndarray] = None,
|
|
22
|
+
):
|
|
23
|
+
"""
|
|
24
|
+
Initialize the filter.
|
|
25
|
+
|
|
26
|
+
Args:
|
|
27
|
+
state: Initial state vector (n x 1)
|
|
28
|
+
covariance: Initial state uncertainty matrix (n x n)
|
|
29
|
+
transition_matrix: Matrix F that defines state progression.
|
|
30
|
+
transition_covariance: Process noise covariance matrix Q.
|
|
31
|
+
measurement_matrix: Matrix H that maps state to measurement.
|
|
32
|
+
measurement_covariance: Measurement noise covariance matrix R.
|
|
33
|
+
"""
|
|
34
|
+
self.state = np.asanyarray(state).astype(float)
|
|
35
|
+
self.covariance = np.asanyarray(covariance).astype(float)
|
|
36
|
+
|
|
37
|
+
self.transition_matrix = transition_matrix
|
|
38
|
+
self.transition_covariance = transition_covariance
|
|
39
|
+
self.measurement_matrix = measurement_matrix
|
|
40
|
+
self.measurement_covariance = measurement_covariance
|
|
41
|
+
|
|
42
|
+
@property
|
|
43
|
+
def x(self) -> np.ndarray:
|
|
44
|
+
"""Current state estimate."""
|
|
45
|
+
return self.state
|
|
46
|
+
|
|
47
|
+
@property
|
|
48
|
+
def P(self) -> np.ndarray:
|
|
49
|
+
"""Current state covariance."""
|
|
50
|
+
return self.covariance
|
|
51
|
+
|
|
52
|
+
@abstractmethod
|
|
53
|
+
def predict(self, dt: float = 1.0, **kwargs) -> np.ndarray:
|
|
54
|
+
"""
|
|
55
|
+
Predict the next state of the system.
|
|
56
|
+
|
|
57
|
+
Args:
|
|
58
|
+
dt: Time step since the last update.
|
|
59
|
+
**kwargs: Additional parameters for specific implementations.
|
|
60
|
+
|
|
61
|
+
Returns:
|
|
62
|
+
The predicted state vector.
|
|
63
|
+
"""
|
|
64
|
+
pass
|
|
65
|
+
|
|
66
|
+
@abstractmethod
|
|
67
|
+
def update(self, measurement: np.ndarray, **kwargs) -> np.ndarray:
|
|
68
|
+
"""
|
|
69
|
+
Update the state estimate with a new measurement.
|
|
70
|
+
|
|
71
|
+
Args:
|
|
72
|
+
measurement: The observed measurement vector.
|
|
73
|
+
**kwargs: Additional parameters for specific implementations.
|
|
74
|
+
|
|
75
|
+
Returns:
|
|
76
|
+
The updated state vector.
|
|
77
|
+
"""
|
|
78
|
+
pass
|
|
79
|
+
|
|
80
|
+
def measure(self, state: Optional[np.ndarray] = None) -> np.ndarray:
|
|
81
|
+
"""
|
|
82
|
+
Map a state to the measurement space.
|
|
83
|
+
|
|
84
|
+
Args:
|
|
85
|
+
state: The state to map. If None, uses internal state.
|
|
86
|
+
|
|
87
|
+
Returns:
|
|
88
|
+
The expected measurement.
|
|
89
|
+
"""
|
|
90
|
+
if state is None:
|
|
91
|
+
state = self.state
|
|
92
|
+
if self.measurement_matrix is None:
|
|
93
|
+
raise ValueError("Measurement matrix H is not defined.")
|
|
94
|
+
return self.measurement_matrix @ state
|
|
95
|
+
|
|
96
|
+
def _check_input(
|
|
97
|
+
self, state: Optional[np.ndarray], measurement: Optional[np.ndarray]
|
|
98
|
+
):
|
|
99
|
+
"""
|
|
100
|
+
Utility for backward compatibility and input validation.
|
|
101
|
+
"""
|
|
102
|
+
if state is None:
|
|
103
|
+
state = self.state
|
|
104
|
+
if measurement is None and self.measurement_matrix is not None:
|
|
105
|
+
measurement = np.random.rand(self.measurement_matrix.shape[0], 1)
|
|
106
|
+
return state, measurement
|
|
@@ -0,0 +1,149 @@
|
|
|
1
|
+
from typing import Optional, Callable
|
|
2
|
+
import numpy as np
|
|
3
|
+
|
|
4
|
+
from kalbee.modules.filters.base import BaseFilter
|
|
5
|
+
|
|
6
|
+
|
|
7
|
+
class ExtendedKalmanFilter(BaseFilter):
|
|
8
|
+
"""
|
|
9
|
+
Extended Kalman Filter (EKF) implementation for non-linear systems.
|
|
10
|
+
|
|
11
|
+
The EKF handles non-linearity by linearizing the system at the current estimate
|
|
12
|
+
using Jacobians.
|
|
13
|
+
"""
|
|
14
|
+
|
|
15
|
+
def __init__(
|
|
16
|
+
self,
|
|
17
|
+
state: np.ndarray,
|
|
18
|
+
covariance: np.ndarray,
|
|
19
|
+
transition_covariance: np.ndarray,
|
|
20
|
+
measurement_covariance: np.ndarray,
|
|
21
|
+
transition_function: Optional[Callable[[np.ndarray, float], np.ndarray]] = None,
|
|
22
|
+
measurement_function: Optional[Callable[[np.ndarray], np.ndarray]] = None,
|
|
23
|
+
transition_jacobian: Optional[Callable[[np.ndarray, float], np.ndarray]] = None,
|
|
24
|
+
measurement_jacobian: Optional[Callable[[np.ndarray], np.ndarray]] = None,
|
|
25
|
+
):
|
|
26
|
+
"""
|
|
27
|
+
Initialize the EKF.
|
|
28
|
+
|
|
29
|
+
Args:
|
|
30
|
+
state: Initial state vector (x).
|
|
31
|
+
covariance: Initial state covariance matrix (P).
|
|
32
|
+
transition_covariance: Process noise covariance (Q).
|
|
33
|
+
measurement_covariance: Measurement noise covariance (R).
|
|
34
|
+
transition_function: Function f(x, dt) returning the predicted state.
|
|
35
|
+
measurement_function: Function h(x) returning the measurement from state.
|
|
36
|
+
transition_jacobian: Function F(x, dt) returning the Jacobian of f.
|
|
37
|
+
measurement_jacobian: Function H(x) returning the Jacobian of h.
|
|
38
|
+
"""
|
|
39
|
+
# We pass None for matrices that are dynamic/non-linear
|
|
40
|
+
super().__init__(
|
|
41
|
+
state=state,
|
|
42
|
+
covariance=covariance,
|
|
43
|
+
transition_covariance=transition_covariance,
|
|
44
|
+
measurement_covariance=measurement_covariance,
|
|
45
|
+
transition_matrix=None,
|
|
46
|
+
measurement_matrix=None,
|
|
47
|
+
)
|
|
48
|
+
|
|
49
|
+
self.transition_function = transition_function
|
|
50
|
+
self.measurement_function = measurement_function
|
|
51
|
+
self.transition_jacobian = transition_jacobian
|
|
52
|
+
self.measurement_jacobian = measurement_jacobian
|
|
53
|
+
|
|
54
|
+
def predict(self, dt: float = 1.0, **kwargs) -> np.ndarray:
|
|
55
|
+
"""
|
|
56
|
+
Predict step for EKF.
|
|
57
|
+
|
|
58
|
+
Args:
|
|
59
|
+
dt: Time step.
|
|
60
|
+
kwargs:
|
|
61
|
+
f: Optional override for transition function.
|
|
62
|
+
F: Optional override for transition Jacobian.
|
|
63
|
+
"""
|
|
64
|
+
# 1. State Prediction: x = f(x, u)
|
|
65
|
+
f_func = kwargs.get("f", self.transition_function)
|
|
66
|
+
if f_func is None:
|
|
67
|
+
raise ValueError(
|
|
68
|
+
"Transition function 'f' must be defined either in init or kwargs."
|
|
69
|
+
)
|
|
70
|
+
|
|
71
|
+
# We assume f takes (state, dt)
|
|
72
|
+
self.state = f_func(self.state, dt)
|
|
73
|
+
|
|
74
|
+
# 2. Covariance Prediction: P = FPF' + Q
|
|
75
|
+
F_func = kwargs.get("F", self.transition_jacobian)
|
|
76
|
+
|
|
77
|
+
# If F is a matrix (constant Jacobian), use it directly
|
|
78
|
+
if isinstance(F_func, np.ndarray):
|
|
79
|
+
F = F_func
|
|
80
|
+
elif callable(F_func):
|
|
81
|
+
F = F_func(self.state, dt)
|
|
82
|
+
else:
|
|
83
|
+
raise ValueError(
|
|
84
|
+
"Transition Jacobian 'F' must be defined either in init or kwargs."
|
|
85
|
+
)
|
|
86
|
+
|
|
87
|
+
Q = self.transition_covariance
|
|
88
|
+
self.covariance = F @ self.covariance @ F.T + Q
|
|
89
|
+
|
|
90
|
+
return self.state
|
|
91
|
+
|
|
92
|
+
def update(self, measurement: np.ndarray, **kwargs) -> np.ndarray:
|
|
93
|
+
"""
|
|
94
|
+
Update step for EKF.
|
|
95
|
+
|
|
96
|
+
Args:
|
|
97
|
+
measurement: Measurement vector z.
|
|
98
|
+
kwargs:
|
|
99
|
+
h: Optional override for measurement function.
|
|
100
|
+
H: Optional override for measurement Jacobian.
|
|
101
|
+
"""
|
|
102
|
+
# 1. Innovation: y = z - h(x)
|
|
103
|
+
h_func = kwargs.get("h", self.measurement_function)
|
|
104
|
+
if h_func is None:
|
|
105
|
+
raise ValueError(
|
|
106
|
+
"Measurement function 'h' must be defined either in init or kwargs."
|
|
107
|
+
)
|
|
108
|
+
|
|
109
|
+
z = measurement
|
|
110
|
+
y = z - h_func(self.state)
|
|
111
|
+
|
|
112
|
+
# 2. Innovation Covariance
|
|
113
|
+
H_func = kwargs.get("H", self.measurement_jacobian)
|
|
114
|
+
|
|
115
|
+
if isinstance(H_func, np.ndarray):
|
|
116
|
+
H = H_func
|
|
117
|
+
elif callable(H_func):
|
|
118
|
+
H = H_func(self.state)
|
|
119
|
+
else:
|
|
120
|
+
raise ValueError(
|
|
121
|
+
"Measurement Jacobian 'H' must be defined either in init or kwargs."
|
|
122
|
+
)
|
|
123
|
+
|
|
124
|
+
R = self.measurement_covariance
|
|
125
|
+
P = self.covariance
|
|
126
|
+
|
|
127
|
+
S = H @ P @ H.T + R
|
|
128
|
+
|
|
129
|
+
# 3. Kalman Gain: K = PH'S^-1
|
|
130
|
+
try:
|
|
131
|
+
K = P @ H.T @ np.linalg.inv(S)
|
|
132
|
+
except np.linalg.LinAlgError:
|
|
133
|
+
# Fallback or error handling for singular matrix
|
|
134
|
+
raise ValueError(
|
|
135
|
+
"Singular matrix encountered in EKF update (inversion of S)."
|
|
136
|
+
)
|
|
137
|
+
|
|
138
|
+
# 4. State Update: x = x + Ky
|
|
139
|
+
self.state = self.state + K @ y
|
|
140
|
+
|
|
141
|
+
# 5. Covariance Update (Joseph form for stability): P = (I - KH)P(I - KH)' + KRK'
|
|
142
|
+
identity = np.eye(len(self.state))
|
|
143
|
+
I_KH = identity - K @ H
|
|
144
|
+
self.covariance = I_KH @ P @ I_KH.T + K @ R @ K.T
|
|
145
|
+
|
|
146
|
+
# Enforce symmetry
|
|
147
|
+
self.covariance = (self.covariance + self.covariance.T) / 2.0
|
|
148
|
+
|
|
149
|
+
return self.state
|
|
@@ -0,0 +1,75 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
|
|
3
|
+
from kalbee.modules.filters.base import BaseFilter
|
|
4
|
+
|
|
5
|
+
|
|
6
|
+
class KalmanFilter(BaseFilter):
|
|
7
|
+
"""
|
|
8
|
+
Standard Linear Kalman Filter implementation.
|
|
9
|
+
"""
|
|
10
|
+
|
|
11
|
+
def __init__(
|
|
12
|
+
self,
|
|
13
|
+
state: np.ndarray,
|
|
14
|
+
covariance: np.ndarray,
|
|
15
|
+
transition_matrix: np.ndarray,
|
|
16
|
+
transition_covariance: np.ndarray,
|
|
17
|
+
measurement_matrix: np.ndarray,
|
|
18
|
+
measurement_covariance: np.ndarray,
|
|
19
|
+
):
|
|
20
|
+
super().__init__(
|
|
21
|
+
state=state,
|
|
22
|
+
covariance=covariance,
|
|
23
|
+
transition_matrix=transition_matrix,
|
|
24
|
+
transition_covariance=transition_covariance,
|
|
25
|
+
measurement_matrix=measurement_matrix,
|
|
26
|
+
measurement_covariance=measurement_covariance,
|
|
27
|
+
)
|
|
28
|
+
|
|
29
|
+
def predict(self, dt: float = 1.0, **kwargs) -> np.ndarray:
|
|
30
|
+
"""
|
|
31
|
+
Predict the next state:
|
|
32
|
+
x = Fx
|
|
33
|
+
P = FPF' + Q
|
|
34
|
+
"""
|
|
35
|
+
F = self.transition_matrix
|
|
36
|
+
Q = self.transition_covariance
|
|
37
|
+
|
|
38
|
+
# In a real KF, F might depend on dt.
|
|
39
|
+
# For simplicity, we assume F is already set or passed in kwargs.
|
|
40
|
+
if "F" in kwargs:
|
|
41
|
+
F = kwargs["F"]
|
|
42
|
+
|
|
43
|
+
self.state = F @ self.state
|
|
44
|
+
self.covariance = F @ self.covariance @ F.T + Q
|
|
45
|
+
|
|
46
|
+
return self.state
|
|
47
|
+
|
|
48
|
+
def update(self, measurement: np.ndarray, **kwargs) -> np.ndarray:
|
|
49
|
+
"""
|
|
50
|
+
Update the state with a measurement:
|
|
51
|
+
y = z - Hx (residual)
|
|
52
|
+
S = HPH' + R (innovation covariance)
|
|
53
|
+
K = P H' S^-1 (Kalman gain)
|
|
54
|
+
x = x + Ky
|
|
55
|
+
P = (I - KH)P
|
|
56
|
+
"""
|
|
57
|
+
z = measurement
|
|
58
|
+
H = self.measurement_matrix
|
|
59
|
+
R = self.measurement_covariance
|
|
60
|
+
P = self.covariance
|
|
61
|
+
x = self.state
|
|
62
|
+
|
|
63
|
+
# Innovation
|
|
64
|
+
y = z - H @ x
|
|
65
|
+
# Innovation covariance
|
|
66
|
+
S = H @ P @ H.T + R
|
|
67
|
+
# Kalman gain
|
|
68
|
+
K = P @ H.T @ np.linalg.inv(S)
|
|
69
|
+
|
|
70
|
+
# Update state and covariance
|
|
71
|
+
self.state = x + K @ y
|
|
72
|
+
identity = np.eye(P.shape[0])
|
|
73
|
+
self.covariance = (identity - K @ H) @ P
|
|
74
|
+
|
|
75
|
+
return self.state
|
|
@@ -0,0 +1,201 @@
|
|
|
1
|
+
Apache License
|
|
2
|
+
Version 2.0, January 2004
|
|
3
|
+
http://www.apache.org/licenses/
|
|
4
|
+
|
|
5
|
+
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
|
6
|
+
|
|
7
|
+
1. Definitions.
|
|
8
|
+
|
|
9
|
+
"License" shall mean the terms and conditions for use, reproduction,
|
|
10
|
+
and distribution as defined by Sections 1 through 9 of this document.
|
|
11
|
+
|
|
12
|
+
"Licensor" shall mean the copyright owner or entity authorized by
|
|
13
|
+
the copyright owner that is granting the License.
|
|
14
|
+
|
|
15
|
+
"Legal Entity" shall mean the union of the acting entity and all
|
|
16
|
+
other entities that control, are controlled by, or are under common
|
|
17
|
+
control with that entity. For the purposes of this definition,
|
|
18
|
+
"control" means (i) the power, direct or indirect, to cause the
|
|
19
|
+
direction or management of such entity, whether by contract or
|
|
20
|
+
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
|
21
|
+
outstanding shares, or (iii) beneficial ownership of such entity.
|
|
22
|
+
|
|
23
|
+
"You" (or "Your") shall mean an individual or Legal Entity
|
|
24
|
+
exercising permissions granted by this License.
|
|
25
|
+
|
|
26
|
+
"Source" form shall mean the preferred form for making modifications,
|
|
27
|
+
including but not limited to software source code, documentation
|
|
28
|
+
source, and configuration files.
|
|
29
|
+
|
|
30
|
+
"Object" form shall mean any form resulting from mechanical
|
|
31
|
+
transformation or translation of a Source form, including but
|
|
32
|
+
not limited to compiled object code, generated documentation,
|
|
33
|
+
and conversions to other media types.
|
|
34
|
+
|
|
35
|
+
"Work" shall mean the work of authorship, whether in Source or
|
|
36
|
+
Object form, made available under the License, as indicated by a
|
|
37
|
+
copyright notice that is included in or attached to the work
|
|
38
|
+
(an example is provided in the Appendix below).
|
|
39
|
+
|
|
40
|
+
"Derivative Works" shall mean any work, whether in Source or Object
|
|
41
|
+
form, that is based on (or derived from) the Work and for which the
|
|
42
|
+
editorial revisions, annotations, elaborations, or other modifications
|
|
43
|
+
represent, as a whole, an original work of authorship. For the purposes
|
|
44
|
+
of this License, Derivative Works shall not include works that remain
|
|
45
|
+
separable from, or merely link (or bind by name) to the interfaces of,
|
|
46
|
+
the Work and Derivative Works thereof.
|
|
47
|
+
|
|
48
|
+
"Contribution" shall mean any work of authorship, including
|
|
49
|
+
the original version of the Work and any modifications or additions
|
|
50
|
+
to that Work or Derivative Works thereof, that is intentionally
|
|
51
|
+
submitted to Licensor for inclusion in the Work by the copyright owner
|
|
52
|
+
or by an individual or Legal Entity authorized to submit on behalf of
|
|
53
|
+
the copyright owner. For the purposes of this definition, "submitted"
|
|
54
|
+
means any form of electronic, verbal, or written communication sent
|
|
55
|
+
to the Licensor or its representatives, including but not limited to
|
|
56
|
+
communication on electronic mailing lists, source code control systems,
|
|
57
|
+
and issue tracking systems that are managed by, or on behalf of, the
|
|
58
|
+
Licensor for the purpose of discussing and improving the Work, but
|
|
59
|
+
excluding communication that is conspicuously marked or otherwise
|
|
60
|
+
designated in writing by the copyright owner as "Not a Contribution."
|
|
61
|
+
|
|
62
|
+
"Contributor" shall mean Licensor and any individual or Legal Entity
|
|
63
|
+
on behalf of whom a Contribution has been received by Licensor and
|
|
64
|
+
subsequently incorporated within the Work.
|
|
65
|
+
|
|
66
|
+
2. Grant of Copyright License. Subject to the terms and conditions of
|
|
67
|
+
this License, each Contributor hereby grants to You a perpetual,
|
|
68
|
+
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
|
69
|
+
copyright license to reproduce, prepare Derivative Works of,
|
|
70
|
+
publicly display, publicly perform, sublicense, and distribute the
|
|
71
|
+
Work and such Derivative Works in Source or Object form.
|
|
72
|
+
|
|
73
|
+
3. Grant of Patent License. Subject to the terms and conditions of
|
|
74
|
+
this License, each Contributor hereby grants to You a perpetual,
|
|
75
|
+
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
|
76
|
+
(except as stated in this section) patent license to make, have made,
|
|
77
|
+
use, offer to sell, sell, import, and otherwise transfer the Work,
|
|
78
|
+
where such license applies only to those patent claims licensable
|
|
79
|
+
by such Contributor that are necessarily infringed by their
|
|
80
|
+
Contribution(s) alone or by combination of their Contribution(s)
|
|
81
|
+
with the Work to which such Contribution(s) was submitted. If You
|
|
82
|
+
institute patent litigation against any entity (including a
|
|
83
|
+
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
|
84
|
+
or a Contribution incorporated within the Work constitutes direct
|
|
85
|
+
or contributory patent infringement, then any patent licenses
|
|
86
|
+
granted to You under this License for that Work shall terminate
|
|
87
|
+
as of the date such litigation is filed.
|
|
88
|
+
|
|
89
|
+
4. Redistribution. You may reproduce and distribute copies of the
|
|
90
|
+
Work or Derivative Works thereof in any medium, with or without
|
|
91
|
+
modifications, and in Source or Object form, provided that You
|
|
92
|
+
meet the following conditions:
|
|
93
|
+
|
|
94
|
+
(a) You must give any other recipients of the Work or
|
|
95
|
+
Derivative Works a copy of this License; and
|
|
96
|
+
|
|
97
|
+
(b) You must cause any modified files to carry prominent notices
|
|
98
|
+
stating that You changed the files; and
|
|
99
|
+
|
|
100
|
+
(c) You must retain, in the Source form of any Derivative Works
|
|
101
|
+
that You distribute, all copyright, patent, trademark, and
|
|
102
|
+
attribution notices from the Source form of the Work,
|
|
103
|
+
excluding those notices that do not pertain to any part of
|
|
104
|
+
the Derivative Works; and
|
|
105
|
+
|
|
106
|
+
(d) If the Work includes a "NOTICE" text file as part of its
|
|
107
|
+
distribution, then any Derivative Works that You distribute must
|
|
108
|
+
include a readable copy of the attribution notices contained
|
|
109
|
+
within such NOTICE file, excluding those notices that do not
|
|
110
|
+
pertain to any part of the Derivative Works, in at least one
|
|
111
|
+
of the following places: within a NOTICE text file distributed
|
|
112
|
+
as part of the Derivative Works; within the Source form or
|
|
113
|
+
documentation, if provided along with the Derivative Works; or,
|
|
114
|
+
within a display generated by the Derivative Works, if and
|
|
115
|
+
wherever such third-party notices normally appear. The contents
|
|
116
|
+
of the NOTICE file are for informational purposes only and
|
|
117
|
+
do not modify the License. You may add Your own attribution
|
|
118
|
+
notices within Derivative Works that You distribute, alongside
|
|
119
|
+
or as an addendum to the NOTICE text from the Work, provided
|
|
120
|
+
that such additional attribution notices cannot be construed
|
|
121
|
+
as modifying the License.
|
|
122
|
+
|
|
123
|
+
You may add Your own copyright statement to Your modifications and
|
|
124
|
+
may provide additional or different license terms and conditions
|
|
125
|
+
for use, reproduction, or distribution of Your modifications, or
|
|
126
|
+
for any such Derivative Works as a whole, provided Your use,
|
|
127
|
+
reproduction, and distribution of the Work otherwise complies with
|
|
128
|
+
the conditions stated in this License.
|
|
129
|
+
|
|
130
|
+
5. Submission of Contributions. Unless You explicitly state otherwise,
|
|
131
|
+
any Contribution intentionally submitted for inclusion in the Work
|
|
132
|
+
by You to the Licensor shall be under the terms and conditions of
|
|
133
|
+
this License, without any additional terms or conditions.
|
|
134
|
+
Notwithstanding the above, nothing herein shall supersede or modify
|
|
135
|
+
the terms of any separate license agreement you may have executed
|
|
136
|
+
with Licensor regarding such Contributions.
|
|
137
|
+
|
|
138
|
+
6. Trademarks. This License does not grant permission to use the trade
|
|
139
|
+
names, trademarks, service marks, or product names of the Licensor,
|
|
140
|
+
except as required for reasonable and customary use in describing the
|
|
141
|
+
origin of the Work and reproducing the content of the NOTICE file.
|
|
142
|
+
|
|
143
|
+
7. Disclaimer of Warranty. Unless required by applicable law or
|
|
144
|
+
agreed to in writing, Licensor provides the Work (and each
|
|
145
|
+
Contributor provides its Contributions) on an "AS IS" BASIS,
|
|
146
|
+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
|
147
|
+
implied, including, without limitation, any warranties or conditions
|
|
148
|
+
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
|
149
|
+
PARTICULAR PURPOSE. You are solely responsible for determining the
|
|
150
|
+
appropriateness of using or redistributing the Work and assume any
|
|
151
|
+
risks associated with Your exercise of permissions under this License.
|
|
152
|
+
|
|
153
|
+
8. Limitation of Liability. In no event and under no legal theory,
|
|
154
|
+
whether in tort (including negligence), contract, or otherwise,
|
|
155
|
+
unless required by applicable law (such as deliberate and grossly
|
|
156
|
+
negligent acts) or agreed to in writing, shall any Contributor be
|
|
157
|
+
liable to You for damages, including any direct, indirect, special,
|
|
158
|
+
incidental, or consequential damages of any character arising as a
|
|
159
|
+
result of this License or out of the use or inability to use the
|
|
160
|
+
Work (including but not limited to damages for loss of goodwill,
|
|
161
|
+
work stoppage, computer failure or malfunction, or any and all
|
|
162
|
+
other commercial damages or losses), even if such Contributor
|
|
163
|
+
has been advised of the possibility of such damages.
|
|
164
|
+
|
|
165
|
+
9. Accepting Warranty or Additional Liability. While redistributing
|
|
166
|
+
the Work or Derivative Works thereof, You may choose to offer,
|
|
167
|
+
and charge a fee for, acceptance of support, warranty, indemnity,
|
|
168
|
+
or other liability obligations and/or rights consistent with this
|
|
169
|
+
License. However, in accepting such obligations, You may act only
|
|
170
|
+
on Your own behalf and on Your sole responsibility, not on behalf
|
|
171
|
+
of any other Contributor, and only if You agree to indemnify,
|
|
172
|
+
defend, and hold each Contributor harmless for any liability
|
|
173
|
+
incurred by, or claims asserted against, such Contributor by reason
|
|
174
|
+
of your accepting any such warranty or additional liability.
|
|
175
|
+
|
|
176
|
+
END OF TERMS AND CONDITIONS
|
|
177
|
+
|
|
178
|
+
APPENDIX: How to apply the Apache License to your work.
|
|
179
|
+
|
|
180
|
+
To apply the Apache License to your work, attach the following
|
|
181
|
+
boilerplate notice, with the fields enclosed by brackets "[]"
|
|
182
|
+
replaced with your own identifying information. (Don't include
|
|
183
|
+
the brackets!) The text should be enclosed in the appropriate
|
|
184
|
+
comment syntax for the file format. We also recommend that a
|
|
185
|
+
file or class name and description of purpose be included on the
|
|
186
|
+
same "printed page" as the copyright notice for easier
|
|
187
|
+
identification within third-party archives.
|
|
188
|
+
|
|
189
|
+
Copyright [yyyy] [name of copyright owner]
|
|
190
|
+
|
|
191
|
+
Licensed under the Apache License, Version 2.0 (the "License");
|
|
192
|
+
you may not use this file except in compliance with the License.
|
|
193
|
+
You may obtain a copy of the License at
|
|
194
|
+
|
|
195
|
+
http://www.apache.org/licenses/LICENSE-2.0
|
|
196
|
+
|
|
197
|
+
Unless required by applicable law or agreed to in writing, software
|
|
198
|
+
distributed under the License is distributed on an "AS IS" BASIS,
|
|
199
|
+
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
200
|
+
See the License for the specific language governing permissions and
|
|
201
|
+
limitations under the License.
|
|
@@ -0,0 +1,117 @@
|
|
|
1
|
+
Metadata-Version: 2.1
|
|
2
|
+
Name: kalbee
|
|
3
|
+
Version: 0.1.0
|
|
4
|
+
Summary: A clean, modular Python implementation of Kalman Filters and estimation algorithms.
|
|
5
|
+
Author-email: Le Duc Minh <minh.leduc.0210@gmail.com>
|
|
6
|
+
Maintainer-email: Le Duc Minh <minh.leduc.0210@gmail.com>
|
|
7
|
+
Project-URL: Homepage, https://github.com/MinLee0210/kalbee
|
|
8
|
+
Project-URL: Repository, https://github.com/MinLee0210/kalbee
|
|
9
|
+
Keywords: kalman-filter,extended-kalman-filter,ekf,state-estimation,sensor-fusion,tracking,alpha-beta-gamma,robotics,signal-processing,python
|
|
10
|
+
Requires-Python: >=3.8
|
|
11
|
+
Description-Content-Type: text/markdown
|
|
12
|
+
License-File: LICENSE
|
|
13
|
+
Requires-Dist: numpy >=1.24.4
|
|
14
|
+
Requires-Dist: pytest >=8.3.5
|
|
15
|
+
Requires-Dist: ruff >=0.14.13
|
|
16
|
+
Requires-Dist: scikit-learn >=1.3.2
|
|
17
|
+
|
|
18
|
+
# kalbee ๐
|
|
19
|
+
|
|
20
|
+
<div align="center">
|
|
21
|
+
<img src="docs/kalbee.png" alt="kalbee logo" width="300"/>
|
|
22
|
+
</div>
|
|
23
|
+
|
|
24
|
+
<br>
|
|
25
|
+
|
|
26
|
+
`kalbee` is a clean, modular Python implementation of Kalman Filters and related estimation algorithms. Designed for simplicity and performance, it provides a standard interface for state estimation in various applications.
|
|
27
|
+
|
|
28
|
+
## โจ Features
|
|
29
|
+
|
|
30
|
+
- **Standard Kalman Filter (KF)**: Linear estimation for dynamic systems.
|
|
31
|
+
- **Extended Kalman Filter (EKF)**: Support for non-linear systems using Jacobians.
|
|
32
|
+
- **Alpha-Beta-Gamma Filter**: Lightweight filtering for kinematic tracking.
|
|
33
|
+
- **AutoFilter**: Factory pattern for easy filter instantiation.
|
|
34
|
+
- **Modular Design**: Easy to extend with new filter types.
|
|
35
|
+
- **NumPy Integration**: Optimized for numerical computations.
|
|
36
|
+
- **Robustness**: Uses Joseph form for numerically stable covariance updates.
|
|
37
|
+
|
|
38
|
+
## ๐ Installation
|
|
39
|
+
|
|
40
|
+
You can install `kalbee` directly from the source:
|
|
41
|
+
|
|
42
|
+
```bash
|
|
43
|
+
git clone https://github.com/MinLee0210/kalbee.git
|
|
44
|
+
cd kalbee
|
|
45
|
+
pip install .
|
|
46
|
+
```
|
|
47
|
+
|
|
48
|
+
Or using `uv` (recommended):
|
|
49
|
+
|
|
50
|
+
```bash
|
|
51
|
+
uv pip install -e .
|
|
52
|
+
```
|
|
53
|
+
|
|
54
|
+
## ๐ ๏ธ Quick Start
|
|
55
|
+
|
|
56
|
+
### 1. Standard Kalman Filter
|
|
57
|
+
|
|
58
|
+
```python
|
|
59
|
+
import numpy as np
|
|
60
|
+
from kalbee.modules.filters import KalmanFilter
|
|
61
|
+
|
|
62
|
+
# Define matrices
|
|
63
|
+
state = np.zeros((2, 1)) # [position, velocity]
|
|
64
|
+
cov = np.eye(2)
|
|
65
|
+
F = np.array([[1, 1], [0, 1]]) # Constant velocity model (dt=1)
|
|
66
|
+
Q = np.eye(2) * 0.01
|
|
67
|
+
H = np.array([[1, 0]]) # We measure position
|
|
68
|
+
R = np.array([[0.1]])
|
|
69
|
+
|
|
70
|
+
# Initialize
|
|
71
|
+
kf = KalmanFilter(state, cov, F, Q, H, R)
|
|
72
|
+
|
|
73
|
+
# Predict & Update
|
|
74
|
+
kf.predict()
|
|
75
|
+
kf.update(np.array([[1.2]]))
|
|
76
|
+
|
|
77
|
+
print(f"Estimated State:\n{kf.x}")
|
|
78
|
+
```
|
|
79
|
+
|
|
80
|
+
### 2. AutoFilter Factory
|
|
81
|
+
|
|
82
|
+
Easily switch between filters using the `AutoFilter` factory:
|
|
83
|
+
|
|
84
|
+
```python
|
|
85
|
+
from kalbee.modules.filters import AutoFilter
|
|
86
|
+
|
|
87
|
+
# Create an EKF
|
|
88
|
+
ekf = AutoFilter.from_filter(
|
|
89
|
+
state, cov, Q, R,
|
|
90
|
+
mode='ekf',
|
|
91
|
+
transition_function=my_f,
|
|
92
|
+
measurement_function=my_h
|
|
93
|
+
)
|
|
94
|
+
```
|
|
95
|
+
|
|
96
|
+
## ๐ Documentation
|
|
97
|
+
|
|
98
|
+
Detailed documentation and examples can be found in the `docs/` directory.
|
|
99
|
+
|
|
100
|
+
- [Getting Started](docs/getting_started.md)
|
|
101
|
+
- [Filtering Logic](docs/filtering_logic.md)
|
|
102
|
+
|
|
103
|
+
## ๐งช Testing
|
|
104
|
+
|
|
105
|
+
Run these commands to test the library:
|
|
106
|
+
|
|
107
|
+
```bash
|
|
108
|
+
uv run pytest tests/
|
|
109
|
+
```
|
|
110
|
+
|
|
111
|
+
## ๐ License
|
|
112
|
+
|
|
113
|
+
This project is licensed under the Apache License 2.0.
|
|
114
|
+
|
|
115
|
+
## ๐ค Contributing
|
|
116
|
+
|
|
117
|
+
Contributions are welcome! Please feel free to submit a Pull Request. Check `TODO.md` for ideas.
|
|
@@ -0,0 +1,13 @@
|
|
|
1
|
+
kalbee/__init__.py,sha256=foTIqbpT5Yo1qR8fhQlkyCiDxSONGo1MM2XVHroG5oo,291
|
|
2
|
+
kalbee/modules/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
3
|
+
kalbee/modules/filters/__init__.py,sha256=nSIgMjzYzMbR3HsV1F1xFSkw3LqN8yIj6xtrlhRQEXU,428
|
|
4
|
+
kalbee/modules/filters/abg_filter.py,sha256=Z62f8L-LGmW1R09wx-Dxngqzl8T9aNAOphwZIaX6LQA,2249
|
|
5
|
+
kalbee/modules/filters/auto_filter.py,sha256=HE3UwVea_r69FkEb7IPPBigKdeqA9nlP6BbH1k7nWi4,1586
|
|
6
|
+
kalbee/modules/filters/base.py,sha256=RX4Gg_eo0tWGsJF6aGYo5S_97f_snrDAVbA4ekf01Iw,3376
|
|
7
|
+
kalbee/modules/filters/ekf_filter.py,sha256=NFzsCMwJ0OjIsAzb8-9Z-W7Qrdvdg4_wdQz7oipeU5Y,5165
|
|
8
|
+
kalbee/modules/filters/kf_filter.py,sha256=nKTa91cI3yI2M6ENmBDTB9s4MAaEIwbS6cbW0JGJyPw,2067
|
|
9
|
+
kalbee-0.1.0.dist-info/LICENSE,sha256=xx0jnfkXJvxRnG63LTGOxlggYnIysveWIZ6H3PNdCrQ,11357
|
|
10
|
+
kalbee-0.1.0.dist-info/METADATA,sha256=15RUjqw4goBSruu_LqPliXGv0iXzsTmPqGiPrly4yxg,3120
|
|
11
|
+
kalbee-0.1.0.dist-info/WHEEL,sha256=WnJ8fYhv8N4SYVK2lLYNI6N0kVATA7b0piVUNvqIIJE,91
|
|
12
|
+
kalbee-0.1.0.dist-info/top_level.txt,sha256=Mi5VYCf5Nlma3QWQxCPLnjUib4u_AH8zvGr864Y8jos,7
|
|
13
|
+
kalbee-0.1.0.dist-info/RECORD,,
|
|
@@ -0,0 +1 @@
|
|
|
1
|
+
kalbee
|