// @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;

            }
Exemplo n.º 2
0
//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);
}
Exemplo n.º 3
0
/// 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;
}
Exemplo n.º 4
0
// 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);
 }
Exemplo n.º 6
0
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;
}