kalma.txt

卡尔曼滤波算法的Vc程序

 //predicted value
  double Xk_1[30][4];
  double P_1[30][16];
  //Kalman gain
  double K[30][8];
  //Obervation
  double Zk[30][2];
double Xk[30][4];  
double P[30][16];
迭代计算：
    //Kalman Filter
   for(ii = 0;ii<list_num;ii++){
//Kalman first: Prediction
   Xk_1[ii][0] = Xk[ii][0] + T*Xk[ii][1];//x
Xk_1[ii][1] = Xk[ii][1];//vx
   Xk_1[ii][2] = Xk[ii][2] + T*Xk[ii][3];//y
   Xk_1[ii][3] = Xk[ii][3];//vy
//Kalman second: Predict error covariance
P_1[ii][0] = P[ii][0]+T*P[ii][4]+T*P[ii][1]+T*T*P[ii][5] + 0.33; 
   P_1[ii][1] = P[ii][1]+T*P[ii][5];
   P_1[ii][2] = P[ii][2]+T*P[ii][6]+T*P[ii][3]+T*T*P[ii][7];
   P_1[ii][3] = P[ii][3]+T*P[ii][7];
   P_1[ii][4] = P[ii][4]+T*P[ii][5];
   P_1[ii][5] = P[ii][5] + 0.33;
   P_1[ii][6] = P[ii][6]+T*P[ii][7];
   P_1[ii][7] = P[ii][7];
   P_1[ii][8] = P[ii][8]+T*P[ii][12]+T*P[ii][9]+T*T*P[ii][13];
   P_1[ii][9] = P[ii][9]+T*P[ii][13];
   P_1[ii][10] = P[ii][10]+T*P[ii][14]+T*P[ii][11]+T*T*P[ii][15] + 0.33;
   P_1[ii][11] = P[ii][11]+T*P[ii][15];
   P_1[ii][12] = P[ii][12]+T*P[ii][13];
   P_1[ii][13] = P[ii][13];
P_1[ii][14] = P[ii][14]+T*P[ii][15];
   P_1[ii][15] = P[ii][15] + 0.33;
//Kalman third: Kalman Gain
   double denominator = 0.0;
   denominator= (P_1[ii][0]+k_noise_co)*(P_1[ii][10]+k_noise_co)-P_1[ii][2]*P_1[ii][8];
K[ii][0]= ((P_1[ii][0]*(P_1[ii][10]+k_noise_co)-P_1[ii][2]*P_1[ii][8])/( denominator ));
   K[ii][1]= ((P_1[ii][2]*(P_1[ii][0]+k_noise_co)-P_1[ii][0]*P_1[ii][2])/( denominator ));
   K[ii][2]= ((P_1[ii][4]*(P_1[ii][10]+k_noise_co)-P_1[ii][6]*P_1[ii][8])/( denominator ));
   K[ii][3]= ((P_1[ii][6]*(P_1[ii][0]+k_noise_co)-P_1[ii][4]*P_1[ii][2])/( denominator ));
   K[ii][4]= ((P_1[ii][8]*(P_1[ii][10]+k_noise_co)-P_1[ii][10]*P_1[ii][8])/( denominator ));
   K[ii][5]= ((P_1[ii][10]*(P_1[ii][0]+k_noise_co)-P_1[ii][8]*P_1[ii][2])/( denominator ));
   K[ii][6]= ((P_1[ii][12]*(P_1[ii][10]+k_noise_co)-P_1[ii][14]*P_1[ii][8])/( denominator ));
   K[ii][7]= ((P_1[ii][14]*(P_1[ii][0]+k_noise_co)-P_1[ii][12]*P_1[ii][2])/( denominator ));
//Kalman fourth:filter
Xk[ii][0] = Xk_1[ii][0] + K[ii][0]*(Zk[ii][0]-Xk_1[ii][0]) + 
     K[ii][1]*(Zk[ii][1]-Xk_1[ii][2]);
   Xk[ii][1] = Xk_1[ii][1] + K[ii][2]*(Zk[ii][0]-Xk_1[ii][0]) + 
             K[ii][3]*(Zk[ii][1]-Xk_1[ii][2]);
   Xk[ii][2] = Xk_1[ii][2] + K[ii][4]*(Zk[ii][0]-Xk_1[ii][0]) + 
            K[ii][5]*(Zk[ii][1]-Xk_1[ii][2]);
   Xk[ii][3] = Xk_1[ii][3] + K[ii][6]*(Zk[ii][0]-Xk_1[ii][0]) + 
           K[ii][7]*(Zk[ii][1]-Xk_1[ii][2]);
//Kalman fifth:filter covariance
   P[ii][0] = (1-K[ii][0])*P_1[ii][0] - K[ii][1]*P_1[ii][8];
   P[ii][1] = (1-K[ii][0])*P_1[ii][1] - K[ii][1]*P_1[ii][9];
   P[ii][2] = (1-K[ii][0])*P_1[ii][2] - K[ii][1]*P_1[ii][10];
   P[ii][3] = (1-K[ii][0])*P_1[ii][3] - K[ii][1]*P_1[ii][11];
   P[ii][4] = -K[ii][2]*P_1[ii][0] + P_1[ii][4] - K[ii][3]*P_1[ii][8];
   P[ii][5] = -K[ii][2]*P_1[ii][1] + P_1[ii][5] - K[ii][3]*P_1[ii][9];
P[ii][6] = -K[ii][2]*P_1[ii][2] + P_1[ii][6] - K[ii][3]*P_1[ii][10];
   P[ii][7] = -K[ii][2]*P_1[ii][3] + P_1[ii][7] - K[ii][3]*P_1[ii][11];
   P[ii][8] = -K[ii][4]*P_1[ii][0] + (1-K[ii][5])*P_1[ii][8];
   P[ii][9] = -K[ii][4]*P_1[ii][1] + (1-K[ii][5])*P_1[ii][9];
   P[ii][10] = -K[ii][4]*P_1[ii][2] + (1-K[ii][5])*P_1[ii][10];
   P[ii][11] = -K[ii][4]*P_1[ii][3] + (1-K[ii][5])*P_1[ii][11];
   P[ii][12] = -K[ii][6]*P_1[ii][0] - K[ii][7]*P_1[ii][8] + P_1[ii][12];
P[ii][13] = -K[ii][6]*P_1[ii][1] - K[ii][7]*P_1[ii][9] + P_1[ii][13];
   P[ii][14] = -K[ii][6]*P_1[ii][2] - K[ii][7]*P_1[ii][10] + P_1[ii][14];
   P[ii][15] = -K[ii][6]*P_1[ii][3] - K[ii][7]*P_1[ii][11] + P_1[ii][15];
        
}//end of for(ii = 0;...)