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

Visual Studio 2010 C++ tips: sscanf_s and fopen_s

-. sscanf vs. sscanf_s

구 버전과 비교하여 파일 및 스트링 관련 함수들에 "_s"가 붙은 함수들이 제공되고 있다. 그러한 함수들 중에는 buffer overflow를 방지하기위한 목적 buffer 사이즈를 명시하도록 규정하고 있다.예를들어strcpy와strcpy_s를 비교해보면, 새로제공되는 함수는 target과source사이에 buffer 사이즈를 입력해야 한다.
"_s"가 붙은 신규함수와 붙지않은 구함수 사이에 함수원형에 있어서 차이점이 없는 경우는, #define을 사용하여 손쉽게 구함수를 신함수로 전환할 수 있으나, 주의해야할 함수가 있다. sscanf_s는 원형이 구 함수인 sscanf와 동일한것 처럼 보이고, 단순히 함수이름만 변경하여도 컴파일 에러가 발생하지 않지만, 실행시 에러가 발생하는 경우가 있다. %c 또는 %s의 포맷으로 데이터를 입력받는 변수의 경우, sizeof()를 사용하여 buffer의 사이즈를 명시하지 않는 경우, 에러가 발생하게 됨으로 주의해야 한다.

-. fopen vs fopen_s 과 fsopen

fopen_s 또한 fopen을 대신하도록 권고되고 있는데, 두 함수사이에 차이점이 있다.
outfile = fopen(filename, "w");
fopen(outfile, filename, "w");
위와 같이 파일을 open하는 경우, fopen과 달리 fopen_s를 사용하는 경우, open된 파일을 sharing이 허용되지 않는다.
따라서, sharing이 허용되도록 하기 위해서는
outfile = fsopen(filename, "w", _SH_DENYNO);
와 같이 fsopen함수와 _SH_DENYNO 플래그를 사용하여 파일을 open해야 한다.

Plane fitting

 

Plane equation:

(1) lx + my + nz =  d

where d is a distance from an origin to the plane,

{l,m,n} is direction consines, and

sqrt(l*l + m*m + n*n)=1.

(2) ax + by + cz = 1

where d presented in (1) can be calculated by 1/sqrt(a*a + b*b + c*c).

(3) Ax + By + Cz = d^2

where (A, B, C) denotes a vector from an origin to the plane, which can be obtained by A = l*d, B=m*d, and C=n*d.

 

For a point cloud, P0, P1, P2, ~ Pn,

Its centroid C, (cx, cy, cz), is on an expected least squre fitted plane.

Therefore, dot product of [l, m, n] and (Pi-C) is zero.

l*xi + m*yi + n*zi = 0

whrer (xi,yi,zi) = Pi

To avoid [l, m, n] are estimated as zero,

a constraint of direction consines is adopted,

l^2 + m^2 + n^2 = 1.0

STL – vector 초간단 설명

vector는 container자료구조중의 하나로서 STL중에서 가장 자주 사용하는 템플릿중 하나이기도 하다. vector의 자료구조는 일반적인 배열과 유사하다. 배열과 상이한 점은 배열의 크기는 일단 정해진 이후에 고정이지만, vector는 동적으로 변할 수 있다는 점이다. 배열의 특징을 더 자세히 살펴보면 데이터가 배열내에 입력된 이후에 특정한 위치에 새로운 데이터의 삽입이 용이하지 않으며, 새로운 데이터의 삽입으로 인해 배열의 크기를 넘어서는 경우 처리가 까다롭다. 특정위치의 데이터를 삭제한 경우에도 빈 공간으로 유지하지 않고, 연속적인 데이터 구조를 유지하기 위해 삭제된 공간을 그 이후의 데이터를 이동하여 채울경우 이 또한 부차적인 처리를 해야하는 불편함이 있다. 반면, 배열은 가장 단순한 형태의 자료구조중 하나이기 때문에 구현이 쉽고, 각 데이터에 대한 random access가 용이하다. vector를 배열의 단점을 어느정도 극복한 형태 자료구조로 여길 수 있다. 즉, 데이터는 순차적으로 저장하지만, 데이터의 개수가 가변적이어도 이에 대응할 수 있다. 그러나, STL의 list와 달리 데이터의 삽입 및 삭제가 용이하지는 않다. 다음의 표는 배열, vector, list에 대한 특징을 간단히 정리한 것이다.

	Array	STL의 vector	STL의 list
가변적 크기	X	O	O
임의 위치의 데이터 삭제/삽입	X	X	O
Random access	O	O	X

<vector 사용법>
#include <vector>//header file
using namespace std;//name space선언
vector<myType> myvector;//vector instance선언
myvector.push_back(newdata1);//vector에 데이터 추가 (뒤에서 추가)
myvector.push_back(newdata2);
int numElements = myvector.size();//저장된 데이터의 갯수
for(int i=0; i<numElements; i++) //일반 배열과 동일한 방식의 데이터 접근 (iterator사용도 가능하다.)
{

myvector.at(i) = myvector.a[i]*2;//at은 특정 위치에 있는 데이터의 reference를 반환한다.

cout<<myvector[i];

}
myType val = (myType)targetvalue;
for(vector<myType>::iterator i=myvector.begin(); i != myvector.end(); )
{

if(*i == val)
{
i = myvector.erase(i);//erase는 iterator i가르키는 데이터를 삭제한 후, 그 다음 데이터를 가르키는 iterator를 반환한다. 따라서, iterator를 증가시키는 프로세스를 필요로 하지 않는다. (주의 요망).
}
else
{
i++;
}

}
myvector.clear();//vector 데이터 모두 삭제

Conversion between CString & std::string

[How to convert CString to std::string]

In Visual Studio 6.0
One used to:

string mystring;
CString myCString("this is a CString");
mystring=(LPCTSTR)myCString;

However, it is not available anymore in Visual Studio 2005.
One should use "CT2CA" do like this:

string mystring;
CString myCString("this is a CString");
CT2CA myCT2CA(myCString);
mystring=myCString;