#include "LSQ_Iteration.h"
using namespace Eigen;
const double PrangeStd = 0.1;   //伪距测距精度
// 转换成时间
const double ClockWeight = 1.0/( (PrangeStd/299792458.0*1e9)*(PrangeStd/299792458.0*1e9) );
LSQ_Iteration::LSQ_Iteration(/* args */)
{
    cnt = 0;
    // InitialTime = 0;
    // InitialClockOffset = 0;
    // Initial = 0;
    Estimate_X.setZero();
    P_old.setZero();
}

LSQ_Iteration::~LSQ_Iteration()
{
}

// 输入Time时刻的钟差ClockOffset
// 进行钟差参数估计
void LSQ_Iteration::InputClockOffset(double Time, double ClockOffset)
{
    if(this->cnt<3)
    {
        this->InitialTime[this->cnt] = Time;
        this->InitialClockOffset[this->cnt] = ClockOffset;
        this->cnt++;
        if (this->cnt==3)
        {
            // 3组数据完成初始化钟差参数
            Matrix3d A;
            Matrix3d P = ClockWeight*Matrix3d::Identity();
            Vector3d l;
            for (size_t i = 0; i < 3; i++)
            {
                double dt = this->InitialTime[i] - this->InitialTime[0];
                A(i, 0) = 1; A(i, 1) = dt; A(i, 2) = dt*dt;
                l(i) = this->InitialClockOffset[i];
            }
            this->Estimate_X = (A.transpose()*P*A).inverse()*(A.transpose()*P*l);
            this->P_old = A.transpose()*P*A;
            this->Initial = 1;
        }
        return;
    }

    // 更新参数
    double dt = Time - this->InitialTime[0];
    // A是列向量
    MatrixXd A;
    A.resize(1, 3);
    A(0) = 1; A(1) = dt; A(2) = dt*dt;
    double l = ClockOffset - PredictionClockOffset(Time);
    double P = ClockWeight;
    MatrixXd N = A.transpose()*P*A;  //更新X的权
    this->P_old = this->P_old + N;
    Vector3d delta_x = this->P_old.inverse()*A.transpose()*P*l;
    this->Estimate_X = this->Estimate_X + delta_x;  //更新状态X
    this->cnt++;
}


// 预测PredictionTime时刻的钟差PredictionClockOffset
// 如果Initial=0，说明不可预测，预测前判断一下
double LSQ_Iteration::PredictionClockOffset(double PredictionTime)
{
    if (this->Initial==0)
    {
        return 0.0;
    }
    double dt = PredictionTime - this->InitialTime[0];
    // A是列向量
    Vector3d A;
    A(0) = 1; A(1) = dt; A(2) = dt*dt;
    double ClockOffset = A.transpose()*this->Estimate_X;
    return ClockOffset;
    
}