#include "KF_Prediction.h"

using namespace Eigen;
const double carrFreq = 2444.97e6;
const double Qw_drift = 1e-5;
const double Qw_driftrate = 1e-5;
const double PrangeStd = 0.1;   //伪距测距精度(m)
const double DopStd = 0.05;   //多普勒精度(Hz)
// 转换成时间
const double ClockNoise = ( (PrangeStd/299792458.0*1e9)*(PrangeStd/299792458.0*1e9) );
const double DopNoise = ( (DopStd/carrFreq*1e9)*(DopStd/carrFreq*1e9) );
const double ErrorThreshold_fact = 0.05;
KF_Prediction::KF_Prediction(/* args */)
{
    cnt = 0;
    Initial = 0;
    Estimate_X.setZero();
    P_old.setIdentity();
    Qw.setZero();
    Qw(1,1) = Qw_drift;
    Qw(2,2) = Qw_driftrate;
    F.setIdentity();
    I.setIdentity();
    H.resize(2, 3);
    H.setZero();
    H(0, 0) = 1;
    H(1, 1) = 1;
    R.setZero();
    R(0, 0) = ClockNoise;
    R(1, 1) = DopNoise;
}

KF_Prediction::~KF_Prediction()
{
}

void KF_Prediction::InputClockOffset(const Input_X &meas)
{
    if(cnt==0){
        Initial = 0;
        this->LastTime = meas.Time;
        Estimate_X[0] = meas.Clock_offset_Prange;
        this->cnt++;
        return;
    }
    if(cnt==1){
        this->LastTime = meas.Time;
        Estimate_X[1] = meas.fre_offset_Doppler/carrFreq*1e9;
        Estimate_X[0] = meas.Clock_offset_Prange;
        this->cnt++;
        return;
    }
    if(cnt>=2){
        Initial = 1;
    }
    // 更新参数
    double dt = meas.Time - this->LastTime;
    this->F(0,1) = dt;  this->F(0,2) = 0.5*dt*dt;
                        this->F(1,2) = dt;
    Vector2d L;
    L(0) = meas.Clock_offset_Prange;
    L(1) = meas.fre_offset_Doppler/carrFreq*1e9;
    // 预测-抗差
    double pre_X = PredictionClockOffset(meas.Time);

    double error = abs(pre_X-meas.Clock_offset_Prange);
    // 误差门限随时间的平方呈正比， a * dt^2
    if(error>5 && error> (ErrorThreshold_fact*dt*dt) ){
        return;
    }

    // kalman filter
    // 时间更新
    Estimate_X = F*Estimate_X;
    Matrix3d P_ = F*P_old*F.transpose() + dt*dt*Qw;
    // // 量测更新
    MatrixXd K = P_*H.transpose()*(H*P_*H.transpose() +R).inverse();
    P_old = (I-K*H)*P_;
    VectorXd V = L - H*Estimate_X;
    Estimate_X = Estimate_X + K*V;
    
    this->LastTime = meas.Time;
    this->cnt++;
}
double KF_Prediction::PredictionClockOffset(const double PredictionTime)
{
    if (this->Initial==0)
    {
        return 0.0;
    }
    double dt = PredictionTime - this->LastTime;
    double val = 0;
#ifdef VCO
    val = Estimate_X(0)/dt + Estimate_X(1) + 0.5*dt*Estimate_X(2);
    // 原单位为ns，转换为s或Hz
    val = val / 1.0e9;
#else
    val = Estimate_X(0) + dt*Estimate_X(1) + 0.5*dt*dt*Estimate_X(2);
#endif
    
    return val;
}