1+ """Symbolic Kalman filter."""
2+
13from sympy import (
24 Function ,
35 Matrix ,
68)
79
810
9- def nth_order_derivative (n , k ):
11+ def nth_order_derivative (n : int , k ):
12+ """Get the nth order derivative of x(k)."""
1013 x = Function ("x" )
1114 if n <= 1 :
1215 return x (k ) - x (k - 1 )
1316 return nth_order_derivative (n - 1 , k ) - nth_order_derivative (n - 1 , k - 1 )
1417
1518
16- def get_all_coeffs (expr , n ):
19+ def get_all_coeffs (expr , n : int ) -> dict :
20+ """Get all coefficients of the expression."""
1721 x = Function ("x" )
1822 k = symbols ("k" )
1923 coeffs = {}
@@ -24,16 +28,17 @@ def get_all_coeffs(expr, n):
2428 return coeffs
2529
2630
27- def Fmij_formula (n , i , j ):
31+ def Fmij_formula (n : int , i : int , j : int ):
32+ """Get the (i,j) element of the matrix."""
2833 k = symbols ("k" )
2934 m = n - 1
3035
31- # denominator знаменатель
36+ # denominator
3237 expr = nth_order_derivative (j , k )
3338 coeffs = get_all_coeffs (expr , j )
3439 den = coeffs [j ]
3540
36- # numerator числитель
41+ # numerator
3742 a = symbols (f"a_{ j + 1 } " )
3843 num = - a
3944 for l in range (0 , m - j ):
@@ -49,7 +54,8 @@ def Fmij_formula(n, i, j):
4954 return simplify (num / den )
5055
5156
52- def get_sympy_filter_matrix (n ):
57+ def get_sympy_filter_matrix (n : int ) -> Matrix :
58+ """Get filter matrix."""
5359 data = []
5460 for i in range (n ):
5561 row = []
@@ -60,7 +66,8 @@ def get_sympy_filter_matrix(n):
6066 return Matrix (data )
6167
6268
63- def refine_filter_matrix (filter_matrix : Matrix , n : int , ar_filter : list ):
69+ def refine_filter_matrix (filter_matrix : Matrix , n : int , ar_filter : list ) -> Matrix :
70+ """Refine filter matrix using AR coefficients."""
6471 filter_matrix_refined = filter_matrix .copy ()
6572 for i in range (1 , n + 1 ):
6673 filter_matrix_refined = filter_matrix_refined .subs ({f"a_{ i } " : ar_filter [i ]})
0 commit comments