diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index cd92a5f630..a721779095 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -96,6 +96,7 @@ class Calibrator(): intrinsics = intrinsics_from_vp(self.vp) new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans)) new_vp = new_vp[:2]/new_vp[2] + new_vp = sanity_clip(new_vp) self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE) self.idx = (self.idx + 1) % BLOCK_SIZE @@ -103,8 +104,7 @@ class Calibrator(): self.block_idx += 1 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED - raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0) - self.vp = sanity_clip(raw_vp) + self.vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0) self.update_status() if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):