Particle-Filter/resampling.c at master · StevenSLXie/Particle-Filter · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
//
//  resampling.c
//  Particle Filter
//
//  Created by Steven S. L. Xie on 12/10/14.
//  Copyright (c) 2014 XIE Shuanglong. All rights reserved.
//

#include <stdio.h>
#include "random_number_gen.h"
#include "particle.h"
#include <stdlib.h>


void cum_sum(struct particle p[], double *sum, int N){

    for(int i =0;i<N;i++)
        sum[i] = 0;

    for(int i=0;i< N;i++){
        for(int j=0;j<= i;j++)
        {
            sum[i] +=  p[j].weight;
        }
        //printf("%f\t", sum[i]);
    }

}

int
multimodal_resampling(struct particle p[], int N)
{
    //double *sum;
    //sum = (double*)calloc(N,sizeof(double));
    double sum[N];


    cum_sum(p,sum,N);

    double x = randu(0,1);
    for(int i = 0;i<N;i++){
        if(sum[i]>x){
            //free(sum);
            return i;
        }
    }
    return -1; //error
}

int stratified_resampling(struct particle p[], int N){
    static double s;

    double sum[N];


    cum_sum(p,sum,N);


    double x = randu(s,s+(1.0/N));
    int index;

    for(int i = 0;i<N;i++){
        if(sum[i]>x){
            //free(sum);
            index = i;
            break;
        }
    }

    //printf("\t%f\t%f\t%f\n",x,sum[0],sum[N-1]);

    s += (double)(1.0/N);

    if((1-s)<0.1/N)
        s = 0;

    return index;

}

int systematic_resampling(struct particle p[], int N){
    static int s;

    s++;

    double sum[N];


    cum_sum(p,sum,N);


    double x = randu(0,(1.0/N));

    x += (float)(s-1.0)/N;  // systematic resampling

    int index;

    for(int i = 0;i<N;i++){
        if(sum[i]>x){
            //free(sum);
            index = i;
            break;
        }
    }

    if(N == s)
        s = 0;

    return index;

}

int
resampling(struct particle p[], int N, resampling_t resampling){
    int sample = 0;
    switch (resampling) {
        case MULTIMODAL:
            sample = multimodal_resampling(p, N);
            break;

        case STRATIFIED:
            sample = stratified_resampling(p,N);
            break;

        case SYSTEMATIC:
            sample = systematic_resampling(p, N);

        default:
            break;
    }
    return sample;
}