Short memo for angle calculation using atan2

Angle and arc-tangent function (in C/C++)

θ = atan2(x,y)
θ' = -(360 - θ)

#1: Bearing and an arc-tangent function

b = atan2(E, N)

Angle between 'N' axis and a vector.

CW is positive.

#2: Rotation angle about an axis and an arc-tangent function

ω = -atan2(x, y)

Angle between 'Y' axis and a vector.

CCW is positive.

Visual Studio 2010 C++ tips: for loop initializer

In Visual Studio 2010, for a loop's initialization variable is not available out of for loop scope, unlike Visual C++ 6.0. Whenever we compile old codes (generated in VC++ 6.0) using newest version, it annoys us. Then, take easy, and change project option to release your blood pressure.

Go to "Project property pages - C/C++ - Language - Force Conformance in For Loop Scope", and select "No (/Zc:forScope-)".

For more information, "http://msdn.microsoft.com/en-us/library/84wcsx8x.aspx".

Visual Studio 2010 C++ tips: Deprecated functions

strcpy, sscanf, fscanf.... etc. There functions are not available anymore in recent Visual Studio .Net C++. Instead of these function, it is recommend to use new functions such as strcpy_s, sscanf_s, fscanf_s ... etc. If you are not pleased to change your old style functions with new ones, you can avoid this problem easily by using this define statement, "#define _CRT_SECURE_NO_DEPRECATE".

For more information about deprecated functions, refer to "http://msdn.microsoft.com/en-us/library/ms235384(v=vs.80).aspx"

Visual Studio 2010 C++ tips: Instead of istream::eatwhite

istream::eatwhite is not supported anymore by recent visual studio C++.

So far, I've used an alternative function implemented by myself. I realized recently that there is a simpler and easier way to avoiding the effort.

In Visual Studio 2010 and 2008, the use of "ws" is the way to skips white space in the stream.

For example, in order to skip white spaces end of each line of a text file.

fstream imufile;

imufile.open(fname, ios::in);

imufile>>record0>>record1>>ws;

For more information,

http://msdn.microsoft.com/query/dev10.query?appId=Dev10IDEF1&l=KO-KR&k=k(%22ISTREAM%2fSTD%3a%3aWS%22);k(%22STD%3a%3aWS%22);k(WS);k(DevLang-%22C%2B%2B%22);k(TargetOS-WINDOWS)&rd=true

Derivation of direct linear transformation for a line scanner

: focal length
: rotation matrix element
: CCD array coordinate (column)
: offset (for the three line scanner)

: First order function of scan line
: Constant (zero order function of scan line)
: Scan line number


 




-. Matrix form

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;
}