KF Filter Sample.py

    def update_filter(self):
        #####################################################################################
        #                                KALMAN STEP                                        #
        #####################################################################################
        t = self.t_index
        #_Y = np.vstack(np.concatenate((self.Inertial_Measurement.flatten(),self.TOF_Measurement.flatten())))
        Y = np.vstack(self.SIM.Measurement)

        # Form estimated error correction of positions form reduced form
        # Y is the positions observed, and is corrected by the X_estimation from the previous round
        X_Est, Y_Est = Form_X_Estimate(self.KF.Reduced_X_Plus, Y, self.params)

        ###################
        # ISOLATED STEP FOR DEBUGGING
        X_True = self.SIM.State.flatten()
        X_Err = X_True-X_Est.flatten()
        # END OF ISOLATION
        ##################

        # Regen design matrices
        H, D = Set_Design_Matrix_H(self.params, X_Est)
        Reduced_H = Set_Reduced_Design_Matrix_H(self.params, H)

        # KE1 (?)
        self.KF.Reduced_X_Minus = np.dot(self.Phi_Reduced, self.KF.Reduced_X_Plus)

        #KE2 (Project error covariance ahead)?
        self.KF.Reduced_P_Minus = np.dot(np.dot(self.Phi_Reduced, self.KF.Reduced_P_Plus), self.Phi_Reduced.T) + self.Q_Reduced

        # KE3 (Compute Kalman Gain)
        #   Analytically this all looks spot on apart from the treatment of Y which should really be Z
        #   K_k = P^-_k . H^T(H . P^-_k . H^T+R)^-1   <<< What is R?
        _HP = np.dot(np.dot(Reduced_H, self.KF.Reduced_P_Minus), Reduced_H.T) # Residual Covariance
        Reduced_K = np.dot(np.dot(self.KF.Reduced_P_Minus, Reduced_H.T), np.linalg.inv(_HP))
        Y_Est = Linearise_Measurements(Y_Est, D)
        Reduced_Y = Form_Reduced_y(Y_Est, Reduced_H, self.params)


        # KE4 (Update Estimate against Measurement)
        ## It appears that the systematic error is introduced via the Reduced Y, meaning, again, that the measurement is wrong.
        self.KF.Reduced_X_Plus = self.KF.Reduced_X_Minus + \
                                 np.dot(Reduced_K, Reduced_Y -
                                                   np.dot(Reduced_H, self.KF.Reduced_X_Minus)
                                 )

        # KE5 (?)
        _I_minus_KH = self.KF.Reduced_I - np.dot(Reduced_K, Reduced_H)
        self.KF.Reduced_P_Plus = np.dot(np.dot(_I_minus_KH, self.KF.Reduced_P_Minus), _I_minus_KH.T)

        # Delta and Variances for Analysis
        self.KF.State_Difference_Estimates[:, t] = np.dot(self.KF.Reduced_H_Out, self.KF.Reduced_X_Plus).T
        _Difference_Covariance = np.dot(np.dot(self.KF.Reduced_H_Out, self.KF.Reduced_P_Plus), self.KF.Reduced_H_Out.T)
        self.KF.State_Difference_Variances[:, t] = np.diag(_Difference_Covariance).T

        # Next predicted state
        self.KF.Estimates[:, t] = X_Est.T
        self.KF.Reduced_States[:, t] = self.KF.Reduced_X_Plus.T
        self.KF.State_Variances[:, t] = np.diag(self.KF.P_Plus).T
        self.KF.Reduced_State_Variances[:, t] = np.diag(self.KF.Reduced_P_Plus).T

        if np.any(np.isnan(self.KF.Reduced_X_Plus)):
            raise(RuntimeError("KF Returning Nan: \n",
                               "rX-:{},\n".format(self.KF.Reduced_X_Minus),
                               "rK: {}\n",format(Reduced_K),
                               "rY: {}\n".format(Reduced_Y),
                               "rH: {}".format(Reduced_H)))

        # Inertial Error Estimate is expected to be [n*ndim,npts]
        Clock_Error_Values, Reduced_Kalman_Filter_Inertial_Error_Estimate, TOF = self.rstate_to_vals(self.KF.Reduced_X_Plus, self.params.Number_Of_Probes)
        #Clock_Offset_Estimate, Clock_Normalised_Frequency_Estimate, Clock_Linear_Frequency_Drift_Estimate = Clock_Error_Values

        self.improved_error_delta = Reduced_Kalman_Filter_Inertial_Error_Estimate
        self.improved_node_positions = self.original_positions + (self.Inertial_Measurement + self.improved_error_delta)
        self.improved_position = self.improved_node_positions[0]
        self.Inertial_Position = self.given_position + (self.Inertial_Measurement)[0]

        # Stuff the logs
        self.improved_pos_log[:,self.t_index] = self.improved_position

        return X_Est