标准粒子滤波算法之四种基本重采样算法

直接上代码了！

#include 
#include 
#include 
#include "resampling.h"

using namespace std;
int main() {

	Resample re;
	int n = 10;
	double weight[10] = {  0.01,0.05,0.01,0.2,0.25,0.16,0.12,0.07,0.13,0.01  };   //权重

	re.Multinomial_rsp(weight, n);
	//re.Stratified_rsp(weight, n);
	//re.Systematic_rsp(weight, n);
	//re.Residual_rsp(weight, n);
	re.~Resample();

	system("pause");
	return 0;
}

#ifndef RESAMPLING_H
#define RESAMPLING_H
using namespace std;

class Resample
{
public:
	Resample(void);
	~Resample(void);
public:

	/*多项式重采样*/
	void Multinomial_rsp(double *wp, int N);
	/*残差重采样*/
	void Residual_rsp(double *wp, int N);
	/*分层重采样*/
	void Stratified_rsp(double *wp, int N);		  
	/*系统重采样*/
	void Systematic_rsp(double *wp, int N);
};
#endif

#include 
#include 
#include 
#include "resampling.h"
using namespace std;

Resample::Resample(void) {
};

Resample::~Resample(void) {
};

/************************************************************************/
/* 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 Resample::Multinomial_rsp(double *wp, int N) {

	/*请求内存*/
	double *c = (double *)malloc(N * sizeof(double));
	double *u = (double *)malloc(N * sizeof(double));
	int *outIndex = (int *)malloc(N * sizeof(int));

	/*计算权重累计和*/
	c[0] = wp[0];
	for (int i = 1; i < N; i++) {
		c[i] = c[i - 1] + wp[i];
	}

	/*生成随机数,生成n个*/
	for (int i = 0; i<N; i++) {
		double u1 = rand();
		double u2 = u1 / RAND_MAX;
		u[i] = u2;
	}

	/*重采样*/
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < N; j++) {
			if (u[i] < c[j]) {
				outIndex[i] = j;
				wp[i] = 1.0 / N;
				break;
			}
		}
	}

	/*显示*/
	for (int i = 0; i < N; i++) {
		cout << outIndex[i] << " ";
	}
	cout << endl;

	/*释放内存*/
	free(c);
	free(u);
	free(outIndex);
}

/************************************************************************/
/* Stratified re-sampling   分层重采样，1996，Kitagawa对多项式重采样进行了改进
/************************************************************************/
void Resample::Stratified_rsp(double *wp, int N) {

	/*请求内存*/
	double *c = (double *)malloc(N * sizeof(double));
	double *u = (double *)malloc(N * sizeof(double));
	int *outIndex = (int *)malloc(N * sizeof(int));

	/*计算累计和*/
	c[0] = wp[0];
	for (int i = 1; i < N; i++) {
		c[i] = c[i - 1] + wp[i];
	}

	/*生成随机数,生成n个*/
	double s = 1.0 / N;
	double u1 = rand();
	double u2 = u1 / RAND_MAX;
	u[0] = u2 * s;
	for (int i = 1; i<N; i++) {
		double t1 = rand();
		double t = t1 / RAND_MAX;
		u[i] = (i + t) * s;
	}

	/*重采样*/
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < N; j++) {
			if (u[i] < c[j]) {
				outIndex[i] = j;
				wp[i] = 1.0 / N;
				break;
			}
		}
	}
	cout << endl;

	/*显示*/
	for (int i = 0; i < N; i++) {
		cout << outIndex[i] << " ";
	}
	cout << endl;

	/*释放内存*/
	free(c);
	free(u);
	free(outIndex);
}
/************************************************************************/
/* Systematic re-sampling   系统重采样 2000,Doucet,在分层重采样算法的基础上提出
*  存在粒子多样性匮乏问题
*  PURPOSE : Performs the re-sampling stage of the SIR in order(number of samples) steps.
*  It uses the systematic sampling scheme of Carpenter and Clifford.
*  - wp = Normalized importance ratios.
*  - outIndex = Re-sampled indices.
*/
/************************************************************************/
void Resample::Systematic_rsp(double *wp, int N)
{
	/*请求内存*/
	double *c = (double *)malloc(N * sizeof(double));
	double *u = (double *)malloc(N * sizeof(double));
	int *outIndex = (int *)malloc(N * sizeof(int));

	/*计算累计和*/
	c[0] = wp[0];
	for (int i = 1; i < N; 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 (int i = 1; i < N; i++) {
		u[i] = (i + u2) * s;
	}

	/*重采样*/
	for (int i = 0; i < N; i++) {
		for (int j = 0; j < N; j++) {
			if (u[i] < c[j]) {
				outIndex[i] = j;
				wp[i] = 1.0 / N;
				break;
			}
		}
	}

	/*显示*/
	for (int i = 0; i < N; i++) {
		cout << outIndex[i] << " ";
	}
	cout << endl;

	/*释放内存*/
	free(c);
	free(u);
	free(outIndex);
}

/************************************************************************/
/* 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 Resample::Residual_rsp(double *wp, int N) {

	/*请求内存*/
	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));
	int *outIndex = (int *)malloc(N * sizeof(int));

	for (int i = 0; i < N; i++) {
		u[i] = floor(M*wp[i]);
		sum += u[i];
	}

	double N_rdn = M - sum;
	for (int i = 0; i < N; i++) {
		ws[i] = (M*wp[i] - floor(M*wp[i])) / N_rdn;
	}

	c[0] = ws[0];
	for (int i = 1; i < N; i++){
		c[i] = c[i - 1] + ws[i];
	}
	int i = 0;
	for (int j = 0; j<N; j++){
		for (int k = 0; k<(int)u[j]; k++){
			outIndex[i] = j;
			wp[i] = 1.0 / M;
			i = i + 1;
		}
	}

	int j = 0;
	while (i<N){
		double t1 = rand();
		double t = t1 / RAND_MAX;
		while (c[j] < t) {
			j = j + 1;
		}
		outIndex[i] = j;
		wp[i] = 1.0 / M;
		i = i + 1;
	}
	for (int k = 0; k < N; k++) {
		cout << outIndex[k] << " ";
	}
	cout << endl;
	free(c);
	free(ws);
	free(u);
}