def update_state_imu(self, state, imu_msg):
        # update 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)

        # assign final state
        state.stamp = imu_msg.header.timestamp
        state.point = self.linear_shift(state.x(), state.y(), avg_heading, state.v(), imu_msg.linear_acceleration.x, dt) # includes x, y
        state.heading = final_orientation # includes heading
        state.ang_acc = ang_acc # includes alpha
        state.ang_vel = Vector.from_Vector3(imu_msg.angular_velocity) # includes omega
        state.lin_vel = lin_vel # includes v
        state.lin_acc = Vector.from_Vector3(imu_msg.linear_acceleration) # includes a

        return state
    def solid_body_translate_lin_acc(self, lin_acc, state, transform):
        # TODO(buckbaskin): check dynamics notes

        # Free Moving Rigid Body Kinematics
        # A-p = A-o + w-dot x R_prime-p + w x (w x R_prime-p)
        # We have A-p (the linear acceleration from the Imu)
        # We are solving for A-o (the acceleration for the baselink frame)
        # angular velocity is consistent across the rigid body (measured at Imu)
        # angular acceleration (w-dot) is interpolated from velocity data
        # R-p is the vector between the two (transform)

        # known: A-p, w-dot, w, R-p
        # solving for: A-o
        # A-o = A-p - w-dot x R-p - w x ( w x R-p )
        Ap = Vector.from_Vector3(lin_acc) # v is a state_model.Vector
        position = transform[0]
        r = Vector.from_tf_position(position)
        Ap = lin_acc
        w_dot = state.alpha()
        w = state.omega()
        
        Ao = (Ap
                .minus(w_dot.cross(r))
                .minus(w.cross(w.cross(r))))
        return Ao.to_Vector3()
    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