Kalman滤波的VC程序

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

马上注册，结交更多好友，享用更多功能，让你轻松玩转社区。

您需要登录才可以下载或查看，没有账号？我要加入

x

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

时间紧，希望能共享一下

发邮件吧：crystal0516@163.com

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

//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;...)

复制代码

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

hao!非常不错！！！

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

好。。。。。

账号		自动登录	找回密码
密码			我要加入

[经典算法] Kalman滤波的VC程序

马上注册，结交更多好友，享用更多功能，让你轻松玩转社区。

本帖被以下淘专辑推荐:

好