crystal111 发表于 2006-7-28 17:06

Kalman滤波的VC程序

谢谢，哪位编过Kalman滤波，我想用它来做跟踪

时间紧，希望能共享一下

发邮件吧：crystal0516@163.com

风花雪月 发表于 2006-8-5 10:20

//predicted value
double Xk_1;
double P_1;
//Kalman gain
double K;
//Obervation
double Zk;
double Xk;
double P;
迭代计算：
    //Kalman Filter
   for(ii = 0;ii<list_num;ii++){
//Kalman first: Prediction
   Xk_1 = Xk + T*Xk;//x
Xk_1 = Xk;//vx
   Xk_1 = Xk + T*Xk;//y
   Xk_1 = Xk;//vy
//Kalman second: Predict error covariance
P_1 = P+T*P+T*P+T*T*P + 0.33;
   P_1 = P+T*P;
   P_1 = P+T*P+T*P+T*T*P;
   P_1 = P+T*P;
   P_1 = P+T*P;
   P_1 = P + 0.33;
   P_1 = P+T*P;
   P_1 = P;
   P_1 = P+T*P+T*P+T*T*P;
   P_1 = P+T*P;
   P_1 = P+T*P+T*P+T*T*P + 0.33;
   P_1 = P+T*P;
   P_1 = P+T*P;
   P_1 = P;
P_1 = P+T*P;
   P_1 = P + 0.33;
//Kalman third: Kalman Gain
   double denominator = 0.0;
   denominator= (P_1+k_noise_co)*(P_1+k_noise_co)-P_1*P_1;
K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
   K= ((P_1*(P_1+k_noise_co)-P_1*P_1)/( denominator ));
//Kalman fourth:filter
Xk = Xk_1 + K*(Zk-Xk_1) +
   K*(Zk-Xk_1);
   Xk = Xk_1 + K*(Zk-Xk_1) +
             K*(Zk-Xk_1);
   Xk = Xk_1 + K*(Zk-Xk_1) +
            K*(Zk-Xk_1);
   Xk = Xk_1 + K*(Zk-Xk_1) +
         K*(Zk-Xk_1);
//Kalman fifth:filter covariance
   P = (1-K)*P_1 - K*P_1;
   P = (1-K)*P_1 - K*P_1;
   P = (1-K)*P_1 - K*P_1;
   P = (1-K)*P_1 - K*P_1;
   P = -K*P_1 + P_1 - K*P_1;
   P = -K*P_1 + P_1 - K*P_1;
P = -K*P_1 + P_1 - K*P_1;
   P = -K*P_1 + P_1 - K*P_1;
   P = -K*P_1 + (1-K)*P_1;
   P = -K*P_1 + (1-K)*P_1;
   P = -K*P_1 + (1-K)*P_1;
   P = -K*P_1 + (1-K)*P_1;
   P = -K*P_1 - K*P_1 + P_1;
P = -K*P_1 - K*P_1 + P_1;
   P = -K*P_1 - K*P_1 + P_1;
   P = -K*P_1 - K*P_1 + P_1;
      
}//end of for(ii = 0;...)

sczhang 发表于 2006-8-9 11:11

hao!非常不错！！！

zhutouzhg 发表于 2006-11-27 10:22

好

好。。。。。
页: [1]
查看完整版本: Kalman滤波的VC程序