// @brief The process equation is used to update the systems state using the process euquations of the system.
// @param sigma_point The sigma point representing a system state.
// @param deltaT The amount of time that has passed since the previous update, in seconds.
// @param measurement The reading from the rate gyroscope in rad/s used to update the orientation.
// @return The new estimated system state.
arma::vec::fixed<IMUModel::size> IMUModel::timeUpdate(const arma::vec::fixed<size>& state, double deltaT) {
arma::vec::fixed<IMUModel::size> newState;
newState = state;
//make a rotation quaternion
const double omega = arma::norm(state.rows(VX, VZ)) + 0.00000000001;
//Negate to compensate for some later mistake.
//deltaT has been negative for a while and has masked an incorrect hack below
const double theta = -omega*deltaT*0.5;
const double sinTheta = sin(theta);
const double cosTheta = cos(theta);
arma::vec vq({cosTheta,state(VX)*sinTheta/omega,state(VY)*sinTheta/omega,state(VZ)*sinTheta/omega});
//calculate quaternion multiplication
//TODO replace with quaternion class
arma::vec qcross = arma::cross( vq.rows(1,3), state.rows(QX,QZ) );
newState(QW) = vq(0)*state(QW) - arma::dot(vq.rows(1,3), state.rows(QX,QZ));
newState(QX) = vq(0)*state(QX) + state(QW)*vq(1) + qcross(0);
newState(QY) = vq(0)*state(QY) + state(QW)*vq(2) + qcross(1);
newState(QZ) = vq(0)*state(QZ) + state(QW)*vq(3) + qcross(2);
return newState;
}
//Odometry
arma::vec RobotModel::predictedObservation(
const arma::vec::fixed<RobotModel::size>& state, const Sensors& sensors) {
// // Velocity in world space:
// arma::mat33 imuRotation = zRotationMatrix(state(kImuOffset));
// arma::vec3 robotHeading_world = imuRotation * arma::mat(sensors.orientation.t()).col(0);
// return WorldToRobotTransform(arma::vec2{0,0}, robotHeading_world.rows(0,1), state.rows(kVX, kVY));
// Velocity in robot space:
return state.rows(kVX, kVY);
}
/// Return the predicted observation of an object at the given position
arma::vec RobotModel::predictedObservation(
const arma::vec::fixed<RobotModel::size>& state,
const arma::vec3& actual_position,
const Sensors& sensors) {
//Rewrite:
// arma::vec2 worldRobotHeading = ImuToWorldHeadingTransform(state(kImuOffset), sensors.robotToIMU);
arma::vec2 worldRobotHeading = ImuToWorldHeadingTransform(state(kImuOffset), sensors.orientation);
auto obs = SphericalRobotObservation(state.rows(kX, kY),
worldRobotHeading,
actual_position);
return obs;
}
// Angle between goals
arma::vec RobotModel::predictedObservation(
const arma::vec::fixed<RobotModel::size>& state,
const std::vector<arma::vec>& actual_positions) {
arma::vec diff_1 = actual_positions[0].rows(0, 1) - state.rows(kX, kY);
arma::vec diff_2 = actual_positions[1].rows(0, 1) - state.rows(kX, kY);
arma::vec radial_1 = cartesianToRadial(diff_1);
arma::vec radial_2 = cartesianToRadial(diff_2);
auto angle_diff = utility::math::angle::difference(radial_1[1], radial_2[1]);
return { std::abs(angle_diff) };
}
// Gyroscope
arma::vec3 IMUModel::predictedObservation(const arma::vec::fixed<size>& state, const MeasurementType::GYROSCOPE&) {
return state.rows(VX, VZ);
}
arma::vec::fixed<RobotModel::size> RobotModel::timeUpdate(
const arma::vec::fixed<RobotModel::size>& state, double deltaT, const Sensors& sensors) {
arma::vec::fixed<RobotModel::size> new_state = state;
// // Velocity in world space:
// new_state.rows(kX,kY) += deltaT * state.rows(kVX,kVY);
// Velocity in robot space:
// arma::vec2 worldRobotHeading = ImuToWorldHeadingTransform(state(kImuOffset), sensors.robotToIMU);
arma::vec2 worldRobotHeading = ImuToWorldHeadingTransform(state(kImuOffset), sensors.orientation);
arma::vec2 worldVelocity = RobotToWorldTransform(arma::vec2{0,0}, worldRobotHeading, state.rows(kVX, kVY));
new_state.rows(kX,kY) += worldVelocity * deltaT;
return new_state;
}
