@@ -257,10 +257,11 @@ def constant_jerk(x, dt, params=None, options=None, r=None, q=None, forwardbackw
257257
258258
259259def robustdiff (x , dt , order , qr_ratio , huberM = 0 ):
260- """Perform outlier-robust differentiation by solving the MAP optimization problem:
261- :math:`\\ min_{\\ {x_n\\ }} \\ sum_{n=0}^{N-1} V(R^{-1/2}(y_n - C x_n)) + \\ sum_{n=1}^{N-1} J(Q^{-1/2}(x_n - A x_{n-1}))`
262- with loss functions :math:`V,J` the :math:`\\ ell_1` norm or Huber loss instead of the :math:`\\ ell_2` norm
263- optimized by RTS smoothing. Uses convex optimization, calls :code:`convex_smooth`.
260+ """Perform outlier-robust differentiation by solving the Maximum A Priori optimization problem:
261+ :math:`\\ min_{\\ {x_n\\ }} \\ sum_{n=0}^{N-1} V(R^{-1/2}(y_n - C x_n)) + \\ sum_{n=1}^{N-1} J(Q^{-1/2}(x_n - A x_{n-1}))`,
262+ where :math:`A,Q,C,R` come from an assumed constant derivative model and :math:`V,J` are the :math:`\\ ell_1` norm or Huber
263+ loss rather than the :math:`\\ ell_2` norm optimized by RTS smoothing. This problem is convex, so this method calls
264+ :code:`convex_smooth`.
264265
265266 :param np.array[float] x: data series to differentiate
266267 :param float dt: step size
@@ -287,19 +288,19 @@ def robustdiff(x, dt, order, qr_ratio, huberM=0):
287288 Q_d = eM [:order + 1 , order + 1 :] @ A_d .T
288289 if np .linalg .cond (Q_d ) > 1e12 : Q_d += np .eye (order + 1 )* 1e-12 # for numerical stability with convex solver. Doesn't change answers appreciably (or at all).
289290
290- x_states = convex_smooth (x , A_d , Q_d , R , C , huberM = huberM ) # outsource solution of the convex optimization problem
291+ x_states = convex_smooth (x , A_d , Q_d , C , R , huberM = huberM ) # outsource solution of the convex optimization problem
291292 return x_states [:, 0 ], x_states [:, 1 ]
292293
293294
294- def convex_smooth (y , A , Q , R , C , huberM = 0 ):
295+ def convex_smooth (y , A , Q , C , R , huberM = 0 ):
295296 """Solve the optimization problem for robust smoothing using CVXPY. Note this currently assumes constant dt
296297 but could be extended to handle variable step sizes by finding discrete-time A and Q for requisite gaps.
297298
298299 :param np.array[float] y: measurements
299300 :param np.array A: discrete-time state transition matrix
300301 :param np.array Q: discrete-time process noise covariance matrix
301- :param np.array R: measurement noise covariance matrix
302302 :param np.array C: measurement matrix
303+ :param np.array R: measurement noise covariance matrix
303304 :param float huberM: radius where quadratic of Huber loss function turns linear. M=0 reduces to the :math:`\\ ell_1` norm.
304305
305306 :return: np.array -- state estimates (N x state_dim)
0 commit comments