Error in RTS equations

[andrewjlock]

Thanks for the useful repository.

It seems there is an error in the RTS smoother equations which causes errors for systems with varying state transition matrices (i.e. EKF).

Replacing it with the formulation using a-priori state and covariance estimates produces the correct solution. I.e., minimal example:

def rts_smoother(x_s, P_s, x_prs, P_prs, F_s):                                                                                                                                                                                                                                                                                                     
    x_rts = [None]*n                                                                                                                                                                                                         
    P_rts = [None]*n                                                                                                                                                                                                         
    x_rts[-1] = x_s[-1]                                                                                                                                                                                                       
    P_rts[-1] = P_s[-1]                                                                                                                                                                                                       
    for k in range(n-2, -1, -1):                                                                                                                                                                                                                                                                                                                                                                                                                         
        C = P_s[k]@(F_s[k+1].T)@np.linalg.inv(P_prs[k+1])                                                                                                                                                                      
        x_rts[k] = x_s[k] + C@(x_rts[k+1] - x_prs[k+1])                                                                                                                                                                       
        P_rts[k] = P_s[k] + C@(P_rts[k+1] - P_prs[k+1])@(C.T)                                                                                                                                                                                                                                                                                                                                                                                                return x_rts, P_rts    

where for an EKF the state transition $F_k( \hat{x}_{k/k-1})$ is calculated for timestep $k$ using the a-priori state estimate of that timestep. This produces the expected RTS smoothing result for me.

Source: Wikipedia