state trans - e-guy/e-guyNotes GitHub Wiki

// status
Eigen::Matrix<double, 6, 1> state;
state.setZero();
state(0, 0) = position(0);
state(1, 0) = position(1);
state(2, 0) = velocity(0);
state(3, 0) = velocity(1);
state(4, 0) = acceleration(0);
state(5, 0) = acceleration(1);

// transition
Eigen::Matrix<double, 6, 6> transition;
transition.setIdentity();
transition(0, 2) = period;
transition(0, 4) = 0.5 * period * period;
transition(1, 3) = period;
transition(1, 5) = 0.5 * period * period;
transition(2, 4) = period;
transition(3, 5) = period;

// Update position, velocity and acceleration
(*state) = transition * (*state);


// state_theta
Eigen::Matrix<double, 2, 1> state_theta;
state_theta.setZero();
state_theta(0, 0) = theta;
state_theta(1, 0) = angular;

// rotation
Eigen::Matrix<double, 2, 2> rotation;
rotation.setIdentity();
rotation(0, 1) = period;

// Update orientation
(*state_theta) = rotation * (*state_theta);

// 1. prediction stage
/// Compute predict states
state = transition * last_state;
/// Compute predicted covariance
Eigen::Matrix4d predict_covariance = Eigen::Matrix4d::Identity() * predict_variance_per_sqrsec_ * time_diff * time_diff;
state_covariance = transition * last_state_covariance * transition.transpose() + predict_covariance;


// 2. measurement update stage
//// Compute kalman gain
const double kVarianceAmplifier = 9.0;
measurement_covariance = measured_velocity_variance_ * direction * direction.transpose() +
    (measured_velocity_variance_ + fabs(measurement.dot(odirection)) * kVarianceAmplifier) * odirection * odirection.transpose();

Eigen::Matrix<double, 4, 2> kalman_gain_matrix =
    static_cast<Eigen::Matrix<double, 4, 2, 0, 4, 2>>(
        state_covariance * observation_transform.transpose() *
        (observation_transform * state_covariance * observation_transform.transpose() + measurement_covariance).inverse()
    );

Eigen::Vector4d state_gain = static_cast<Eigen::Matrix<double, 4, 1, 0, 4, 1>>(
          kalman_gain_matrix * (measurement - observation_transform * state)
    );

// 3. gain adjustment and estimate posterior
state = state + state_gain;
state_covariance = (Eigen::Matrix4d::Identity() - kalman_gain_matrix * observation_transform) * state_covariance;

//
//
// std::cout<<"observation"<<observe.transpose()<<std::endl;
lst_state_=A_*lst_state_;
// std::cout<<"P_ matrix: "<<P_<<std::endl;
P_=A_*P_*A_.transpose()+Q_;
// std::cout<<"P_ matrix: "<<P_<<std::endl;
Eigen::MatrixXf S = H_*P_*H_.transpose()+R_;
// std::cout<<"S matrix: "<<S<<std::endl;
Eigen::MatrixXf K = (P_*H_.transpose())*S.inverse();
// std::cout<<"lst_state_: "<<lst_state_.transpose()<<std::endl;
// std::cout<<"K matrix: "<<K<<std::endl;
// std::cout<<"correnction: "<<(observe-(H_*lst_state_)).transpose()<<std::endl;
lst_state_=lst_state_+K*(observe-(H_*lst_state_));
// std::cout<<"correct lst_state_: "<<lst_state_.transpose()<<std::endl;
P_=P_*(I_-(K*H_));
// std::cout<<"lst_state_: "<<lst_state_<<std::endl;
// std::memcpy(filted_loc,lst_state_.ptr<float>(),sizeof(float)*dim_);
velocity(0)=lst_state_(2,0);
velocity(1)=lst_state_(3,0);