kf.py 文件源码

def correct(self, r):
        """Perform correction (measurement) update."""
        zhat, H = r.mfn(self.x)
        dz = r.z - zhat
        S = H @ self.P @ H.T + r.R
        SI = inv(S)
        K = self.P @ H.T @ SI
        self.x += K @ dz
        self.P -= K @ H @ self.P

        score = dz.T @ SI @ dz / 2.0 + ln(2 * pi * sqrt(det(S)))

        self._calc_bbox()

        return float(score)