UPF重采样算法

Multinomial re-sampling 多项式重采样，Residual re-sampling 残差重采样，Stratified re-sampling 分层重采样，Systematic re-sampling 系统重采样 ， regularized re- sampling 正则重采样算法

#include 
#include 
#include 
#include “resampling.h“
//  重采样算法，参考：博士论文《粒子滤波算法及其应用研究》
//
// void re_sampling（int* outIndexdouble *wp int N）
// {
// 	int ij;
// 	double *c=（double *）malloc（N*sizeof（double））;
// 	double *u=（double *）malloc（N*sizeof（double））;
// 	c[0]=wp[0];
// 	for（i=1;i// 		c[i]=c[i-1]+wp[i];
// 
// 	double s=1.0/N;
// 	double u1=rand（）;
// 	double u2=u1/RAND_MAX;
// 	u[0]=u2*s;
// 	for（i=1;i// 		u[i]=u[i-1]+s;
// 	i=0;j=0;
// 	while（i// 	{
// 		if （u[i]// 		{
// 			outIndex[i]=j;
// 			wp[i]=s;
// 			i=i+1;
// 		}
// 		else
// 			j=j+1;
// 	}
// 	free（u）;
// 	free（c）;
// }
/************************************************************************/
/* Multinomial re-sampling   多项式重采样 ，1993，Gordon计算量大
 * PURPOSE : Performs the re-sampling stage of the SIR in order（number of samples） steps.
 *  - wp = Normalized importance ratios.
 *  - outIndex = Re-sampled indices.
 */
/************************************************************************/
void Multinomial_rsp（int* outIndexdouble *wp int N）
{
	int ij;
	double *c=（double *）malloc（N*sizeof（double））;
	c[0]=wp[0];
	for（i=1;i		c[i]=c[i-1]+wp[i];	
	
	i=0;
	while（i	{
		double u1=rand（）;
		double u=u1/RAND_MAX;
		j=0;
		while （c[j]			j=j+1;
		outIndex[i]=j;
		wp[i]=1.0/N;
		i=i+1;
	}
	free（c）;
}
/************************************************************************/
/* Residual re-sampling   残差重采样 1993 Liu在多项式重采样的基础上提出，
 * 计算量小于多项式重采样算法
 * PURPOSE : Performs the re-sampling stage of the SIR in order（number of samples） steps. 
 * It uses Liu‘s residual re-sampling algorithm and Niclas‘ magic line.
 *  - wp = Normalized importance ratios.
 *  - outIndex = Re-sampled indices.
*/
/************************************************************************/
void Residual_rsp（int* outIndexdouble *wp int N）
{
	int ijk;
	double M=（double） N;
	double sum=0.0;
	double *u=（double *）malloc（N*sizeof（double））;
	double *ws=（double *）malloc（N*sizeof（double））;
	double *c=（double *）malloc（N*sizeof（double））;
	for（i=0;i	{
		u[i]=floor（M*wp[i]）;
		sum+=u[i];
	}
	double N_rdn=M-sum;
	for（i=0;i		ws[i]=（M*wp[i]-floor（M*wp[i]））/N_rdn;

	c[0]=ws[0];
	for（i=1;i		c[i]=c[i-1]+ws[i];

	i=0;
	for （j=0;j	{
		for（k=0;k<（int）u[j];k++）
		{
			outIndex[i]=j;
			wp[i]=1.0/M;
			i=i+1;
		}
	}

	j=0;
	while（i	{
		double t1=rand（）;
		double t=t1/RAND_MAX;
		while（c[j]			j=j+1;
		outIndex[i]=j;
		wp[i]=1.0/M;
		i=i+1;
	}
	free（c）;
	free（ws）;
	free（u）;	
}
/************************************************************************/
/* Stratified re-sampling   分层重采样，1996，Kitagawa对多项式重采样进行了改进
*/
/************************************************************************/
void Stratified_rsp（int* outIndexdouble *wp int N）
{
	int ij;
	double s=1.0/N;
	double *c=（double *）malloc（N*size

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        328  2012-03-28 15:42  resampling.h

     文件       4704  2012-03-28 16:07  resampling.cpp

     文件    6583416  2012-03-27 15:55  粒子滤波算法及其应用研究.kdh

----------- ---------  ---------- -----  ----

              6588448                    3