def update_state_init(self, state, imu_msg): # average state with new imu_information # Renamed state vars # (time - time) => duration dt = (imu_msg.header.stamp - state.timestamp()).to_sec() final_orientation = self.quat_to_euler(imu_msg.orientation) # jerk twerk = self.twerk(state.theta(), final_orientation, state.omega(), state.alpha(), dt) # intermediate vars avg_heading = self.theta_avg(twerk, state.theta(), state.omega(), state.alpha(), dt) ang_acc = self.ang_acc(twerk, state.alpha(), dt) # lin_vel((a_measured, v0, dt)) lin_vel = self.lin_vel(imu_msg.linear_acceleration.x, state.v(), dt) # average final state # so the values collected during the init are a moving average # this all should be pretty stable around 0, because the robot should be stopped # also, all state models should use this to zero-out their models state.point = Vector.average(state.point, self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt)) # includes x, y state.heading = state.heading/2 + final_orientation/2 # includes heading state.ang_acc = Vector.average(state.ang_acc, ang_acc) # includes alpha state.ang_vel = Vector.average(state.ang_vel, Vector.from_Vector3(imu_msg.angular_velocity)) # includes omega state.lin_vel = Vector.average(state.lin_vel, lin_vel) # includes v state.lin_acc = Vector.average(state.lin_acc, Vector.from_Vector3(imu_msg.linear_acceleration)) # includes a return state