Simple test: Kalman filter and least square filter for a dynamic system

Kalman filter is one of most popular signal filter. As well known, the fundamental of Kalman Filter and Least Square Method is same. Here is a sample for a dynamic system, where filtering is conducted by KF and LS approaches.

double rand_num()
{    
    return 2*((rand()/(double)RAND_MAX) - 0.5);
}
int KF_LS_Test()
{
    double pos[NUMDATA];
    double pos_t[NUMDATA];
   
    srand((unsigned int)time(NULL));
    double noise = 0.09;
   
    for(int t=0; t<NUMDATA; t++)
    {
        double p = (double)cosf(2.0f*(float)t/(float)NUMDATA*(float)acos(-1.0));
       
        pos_t[t] = p;
        pos[t] = p + rand_num()*noise;
    }
    //Initial values
    CSMMatrix<double> x0(1,1,0.0);
    CSMMatrix<double> P0(1,1,0.0);
    CSMMatrix<double> xk0(1,1,0.0);
    CSMMatrix<double> Pk0(1,1,0.0);
    CSMMatrix<double> xk1_(1,1,0.0);
    CSMMatrix<double> Pk1_(1,1,0.0);
    CSMMatrix<double> xk1(1,1,0.0);
    CSMMatrix<double> Pk1(1,1,0.0);
    CSMMatrix<double> Qk(1,1);
    Qk(0,0) = noise*noise*0.01;
    CSMMatrix<double> Rk(1,1);
    Rk(0,0) = noise*noise;
    CSMMatrix<double> K(1,1,1.0);
    CSMMatrix<double> I(1,1,1.0);
    xk1(0,0) = pos_t[0] + rand_num()*noise;
    Pk1(0,0) = 0.0;//1.0e99;
    fstream outfile;
    outfile.open(_T("C:\\Users\\kibang\\Desktop\\KFtestout.txt"), ios::out);
    _tprintf(_T("R = %lf\n"),Rk(0,0));
    _tprintf(_T("Q = %lf\n"),Qk(0,0));
    _tprintf(_T("@time = %d\n"),0);
    _tprintf(_T("x = %lf\n"),xk1(0,0));
    _tprintf(_T("P = %lf\n"),Pk1(0,0));
    int count = 0;
    CSMMatrix<double> Var_x_hat(1,1);
    double s_hat;
    CSMMatrix<double> X, H(1,1,1.0), L(1,1), L2(1,1), W(1,1), N(1,1,0.0), C(1,1,0.0);
    W(0,0) = 1.0/Rk(0,0);
    //Var_x_hat(0,0) = Pk1(0,0);
    Var_x_hat(0,0) = Qk(0,0);
    double VTV = 0;
   
    for(int t=0; t<NUMDATA; t++)
    {
        //
        //LS approach
        //
        count ++;
        L(0,0) = pos[t];
        L2(0,0) = xk1(0,0);
        Var_x_hat = Qk;
        CSMMatrix<double> Ni = ( H.Transpose() % W % H + Var_x_hat.Inverse() );
        //CSMMatrix<double> Ni = ( H.Transpose() % W % H);
        CSMMatrix<double> Ci = (H.Transpose() % W % L + Var_x_hat.Inverse() % L2);
        //N = N + Ni; C = C + Ci;
        N = Ni; C = Ci;
        X = N.Inverse() % C;
        //VTV += (X(0,0) - pos[t])*(X(0,0) - pos[t])*W(0,0);
        VTV = (X(0,0) - pos[t])*(X(0,0) - pos[t])*W(0,0);
        s_hat = sqrt(VTV/count);
        Var_x_hat = N.Inverse()*(s_hat*s_hat);
        //
        //KF approach
        //
        _tprintf(_T("@time = %d\n"),t+1);
        _tprintf(_T("True x = %lf\n"),0.0);
        xk0 = xk1;
        Pk0 = Pk1;
        //Predict
        xk1_ = xk0;
        Pk1_ = Pk0 + Qk;
        _tprintf(_T("Predicted x = %lf\n"),xk1_(0,0));
        _tprintf(_T("Predicted P = %lf\n"),Pk1_(0,0));
        //Kalman gain
        K = Pk1_ % (Pk1_ + Rk).Inverse();
        _tprintf(_T("Measured x = %lf\n"),pos[t]);
        //Update state using measurement
        CSMMatrix<double> Pos(1,1,pos[t]);
        xk1 = xk1_ + K % (Pos - xk1_);
        //Update error covariance
        Pk1 = (I - K)%Pk1_;
       
        _tprintf(_T("Updated x = %lf\n"),xk1(0,0));
        _tprintf(_T("Updated P = %lf\n"),Pk1(0,0));
        _tprintf(_T("Adjusted(LS) P = %lf\n"),X(0,0));
        outfile<<t+1<<"\t"<<pos_t[t]<<"\t"<<xk1_(0,0)<<"\t"<<xk1(0,0)<<"\t"<<X(0,0)<<"\t"<<pos[t]<<endl;
    }
    outfile.close();
    return 0;
}