88except ImportError : pass
99
1010
11- def kalman_filter (y , _t , xhat0 , P0 , A , Q , C , R , B = None , u = None , save_P = True ):
11+ def kalman_filter (y , dt_or_t , xhat0 , P0 , A , Q , C , R , B = None , u = None , save_P = True ):
1212 """Run the forward pass of a Kalman filter, with regular or irregular step size.
1313
1414 :param np.array y: series of measurements, stacked along axis 0.
15- :param float or array[float] _t : This function supports variable step size. This parameter is either the constant
15+ :param float or array[float] dt_or_t : This function supports variable step size. This parameter is either the constant
1616 step size if given as a single float, or sample locations if given as an array of same length as :code:`x`.
1717 :param np.array xhat0: a priori guess of initial state at the time of the first measurement
1818 :param np.array P0: a priori guess of state covariance at the time of first measurement (often identity matrix)
@@ -39,7 +39,7 @@ def kalman_filter(y, _t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
3939 P_pre = np .empty ((N ,m ,m )) # _post = a posteriori combinations of all information available at a step
4040 P_post = np .empty ((N ,m ,m ))
4141 # determine some things ahead of the loop
42- equispaced = np .isscalar (_t )
42+ equispaced = np .isscalar (dt_or_t )
4343 control = isinstance (B , np .ndarray ) and isinstance (B , np .ndarray ) # whether there is a control input
4444 if equispaced :
4545 An , Qn , Bn = A , Q , B # in this case only need to assign once
@@ -53,7 +53,7 @@ def kalman_filter(y, _t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
5353 P_ = P0
5454 else :
5555 if not equispaced :
56- dt = _t [n ] - _t [n - 1 ]
56+ dt = dt_or_t [n ] - dt_or_t [n - 1 ] # vector of t in this case
5757 eM = expm (M * dt ) # form discrete-time matrices
5858 An = eM [:m ,:m ] # upper left block
5959 Qn = eM [:m ,m :] @ An .T # upper right block
@@ -77,10 +77,10 @@ def kalman_filter(y, _t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
7777 return xhat_post if not save_P else (xhat_pre , xhat_post , P_pre , P_post )
7878
7979
80- def rts_smooth (_t , A , xhat_pre , xhat_post , P_pre , P_post , compute_P_smooth = True ):
80+ def rts_smooth (dt_or_t , A , xhat_pre , xhat_post , P_pre , P_post , compute_P_smooth = True ):
8181 """Perform Rauch-Tung-Striebel smoothing, using information from forward Kalman filtering.
8282
83- :param float or array[float] _t : This function supports variable step size. This parameter is either the constant
83+ :param float or array[float] dt_or_t : This function supports variable step size. This parameter is either the constant
8484 step size if given as a single float, or sample locations if given as an array of same length as the state histories.
8585 :param np.array A: state transition matrix, in discrete time if constant dt, in continuous time if variable dt
8686 :param np.array xhat_pre: a priori estimates of xhat from a kalman_filter forward pass
@@ -96,23 +96,23 @@ def rts_smooth(_t, A, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=True)
9696 xhat_smooth = np .empty (xhat_post .shape ); xhat_smooth [- 1 ] = xhat_post [- 1 ] # preallocate arrays
9797 if compute_P_smooth : P_smooth = np .empty (P_post .shape ); P_smooth [- 1 ] = P_post [- 1 ]
9898
99- equispaced = np .isscalar (_t ) # I'd rather not call isinstance in a loop when it's avoidable
99+ equispaced = np .isscalar (dt_or_t ) # to avoid calling this in the loop
100100 if equispaced : An = A # in this case only assign once, outside the loop
101101 for n in range (xhat_pre .shape [0 ]- 2 , - 1 , - 1 ):
102- if not equispaced : An = expm (A * (_t [n + 1 ] - _t [n ])) # state transition from n to n+1, per the paper
102+ if not equispaced : An = expm (A * (dt_or_t [n + 1 ] - dt_or_t [n ])) # state transition from n to n+1
103103 C_RTS = P_post [n ] @ An .T @ np .linalg .inv (P_pre [n + 1 ]) # the [n+1]th index holds _{n+1|n} info
104104 xhat_smooth [n ] = xhat_post [n ] + C_RTS @ (xhat_smooth [n + 1 ] - xhat_pre [n + 1 ]) # The original authors use C, not to be confused
105105 if compute_P_smooth : P_smooth [n ] = P_post [n ] + C_RTS @ (P_smooth [n + 1 ] - P_pre [n + 1 ]) @ C_RTS .T # with the measurement matrix
106106
107107 return xhat_smooth if not compute_P_smooth else (xhat_smooth , P_smooth )
108108
109109
110- def rtsdiff (x , _t , order , log_qr_ratio , forwardbackward ):
110+ def rtsdiff (x , dt_or_t , order , log_qr_ratio , forwardbackward ):
111111 """Perform Rauch-Tung-Striebel smoothing with a naive constant derivative model. Makes use of :code:`kalman_filter`
112112 and :code:`rts_smooth`, which are made public. :code:`constant_X` methods in this module call this function.
113113
114114 :param np.array[float] x: data series to differentiate
115- :param float or array[float] _t : This function supports variable step size. This parameter is either the constant
115+ :param float or array[float] dt_or_t : This function supports variable step size. This parameter is either the constant
116116 step size if given as a single float, or data locations if given as an array of same length as :code:`x`.
117117 :param int order: which derivative to stabilize in the constant-derivative model
118118 :param log_qr_ratio: the log of the process noise level divided by the measurement noise level, because,
@@ -124,8 +124,8 @@ def rtsdiff(x, _t, order, log_qr_ratio, forwardbackward):
124124 :return: - **x_hat** (np.array) -- estimated (smoothed) x
125125 - **dxdt_hat** (np.array) -- estimated derivative of x
126126 """
127- if not np .isscalar (_t ) and len (x ) != len (_t ):
128- raise ValueError ("If `_t ` is given as array-like, must have same length as `x`." )
127+ if not np .isscalar (dt_or_t ) and len (x ) != len (dt_or_t ):
128+ raise ValueError ("If `dt_or_t ` is given as array-like, must have same length as `x`." )
129129 x = np .asarray (x ) # to flexibly allow array-like inputs
130130
131131 q = 10 ** int (log_qr_ratio / 2 ) # even-ish split of the powers across 0
@@ -138,13 +138,13 @@ def rtsdiff(x, _t, order, log_qr_ratio, forwardbackward):
138138 P0 = 100 * np .eye (order + 1 ) # See #110 for why this choice of P0
139139 xhat0 = np .zeros (A .shape [0 ]); xhat0 [0 ] = x [0 ] # The first estimate is the first seen state. See #110
140140
141- if np .isscalar (_t ):
142- eM = expm (np .block ([[A , Q ],[np .zeros (A .shape ), - A .T ]]) * _t ) # form discrete-time versions
141+ if np .isscalar (dt_or_t ):
142+ eM = expm (np .block ([[A , Q ],[np .zeros (A .shape ), - A .T ]]) * dt_or_t ) # form discrete-time versions
143143 A = eM [:order + 1 ,:order + 1 ]
144144 Q = eM [:order + 1 ,order + 1 :] @ A .T
145145
146- xhat_pre , xhat_post , P_pre , P_post = kalman_filter (x , _t , xhat0 , P0 , A , Q , C , R ) # noisy x are the "y" in Kalman-land
147- xhat_smooth = rts_smooth (_t , A , xhat_pre , xhat_post , P_pre , P_post , compute_P_smooth = False )
146+ xhat_pre , xhat_post , P_pre , P_post = kalman_filter (x , dt_or_t , xhat0 , P0 , A , Q , C , R ) # noisy x are the "y" in Kalman-land
147+ xhat_smooth = rts_smooth (dt_or_t , A , xhat_pre , xhat_post , P_pre , P_post , compute_P_smooth = False )
148148 x_hat_forward = xhat_smooth [:, 0 ] # first dimension is time, so slice first element at all times
149149 dxdt_hat_forward = xhat_smooth [:, 1 ]
150150
@@ -153,11 +153,11 @@ def rtsdiff(x, _t, order, log_qr_ratio, forwardbackward):
153153
154154 xhat0 [0 ] = x [- 1 ] # starting from the other end of the signal
155155
156- if np .isscalar (_t ): A = np .linalg .inv (A ) # discrete time dynamics are just the inverse
157- else : _t = _t [::- 1 ] # in continuous time, reverse the time vector so dts go negative
156+ if np .isscalar (dt_or_t ): A = np .linalg .inv (A ) # discrete time dynamics are just the inverse
157+ else : dt_or_t = dt_or_t [::- 1 ] # in continuous time, reverse the time vector so dts go negative
158158
159- xhat_pre , xhat_post , P_pre , P_post = kalman_filter (x [::- 1 ], _t , xhat0 , P0 , A , Q , C , R )
160- xhat_smooth = rts_smooth (_t , A , xhat_pre , xhat_post , P_pre , P_post , compute_P_smooth = False )
159+ xhat_pre , xhat_post , P_pre , P_post = kalman_filter (x [::- 1 ], dt_or_t , xhat0 , P0 , A , Q , C , R )
160+ xhat_smooth = rts_smooth (dt_or_t , A , xhat_pre , xhat_post , P_pre , P_post , compute_P_smooth = False )
161161 x_hat_backward = xhat_smooth [:, 0 ][::- 1 ] # the result is backwards still, so reverse it
162162 dxdt_hat_backward = xhat_smooth [:, 1 ][::- 1 ]
163163
0 commit comments