粒子フィルターのC言語実装

参考文献

時系列解析 ―自己回帰型モデル・状態空間モデル・異常検知―
著者 島田 直希
発売日 2019/09/11

準備

オンラインコンパイラを使用します。

ソースコード

sample.c

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define NUM_TIMESTEPS 50
#define NUM_PARTICLES 100

// 隠れマルコフモデルの定義
double transition_prob[2][2] = {
    {0.7, 0.3},
    {0.3, 0.7}
};

double observation_prob[2][2] = {
    {0.9, 0.1},
    {0.2, 0.8}
};

// シミュレーション用関数
void generate_data(int num_timesteps, int *true_states, int *observations) {
    int true_state = 0;

    for (int t = 0; t < num_timesteps; t++) {
        true_state = (rand() < transition_prob[true_state][0] * RAND_MAX) ? 0 : 1;
        int observation = (rand() < observation_prob[true_state][0] * RAND_MAX) ? 0 : 1;
        true_states[t] = true_state;
        observations[t] = observation;
    }
}

// 粒子フィルターの実装
void particle_filter(int *observations, int num_timesteps, int num_particles, int particles[NUM_TIMESTEPS][NUM_PARTICLES]) {
    // 初期化
    for (int i = 0; i < num_particles; i++) {
        particles[0][i] = (rand() < 0.5 * RAND_MAX) ? 0 : 1;
    }

    for (int t = 0; t < num_timesteps; t++) {
        double weights[NUM_PARTICLES];
        double total_weight = 0.0;

        for (int i = 0; i < num_particles; i++) {
            weights[i] = observation_prob[particles[t][i]][observations[t]];
            total_weight += weights[i];
        }

        for (int i = 0; i < num_particles; i++) {
            weights[i] /= total_weight;
        }

        int new_particles[NUM_PARTICLES];
        for (int i = 0; i < num_particles; i++) {
            double r = ((double)rand() / RAND_MAX);
            double cumulative_sum = 0.0;
            for (int j = 0; j < num_particles; j++) {
                cumulative_sum += weights[j];
                if (r <= cumulative_sum) {
                    new_particles[i] = particles[t][j];
                    break;
                }
            }
        }

        for (int i = 0; i < num_particles; i++) {
            particles[t][i] = new_particles[i];
        }

        if (t < num_timesteps - 1) {
            for (int i = 0; i < num_particles; i++) {
                particles[t+1][i] = (rand() < transition_prob[particles[t][i]][0] * RAND_MAX) ? 0 : 1;
            }
        }
    }
}

int main() {
    srand(time(0));

    // データ生成
    int true_states[NUM_TIMESTEPS];
    int observations[NUM_TIMESTEPS];
    generate_data(NUM_TIMESTEPS, true_states, observations);

    // 粒子フィルターの実行
    int particles[NUM_TIMESTEPS][NUM_PARTICLES];
    particle_filter(observations, NUM_TIMESTEPS, NUM_PARTICLES, particles);

    // 結果の表示
    double estimated_states[NUM_TIMESTEPS];
    for (int t = 0; t < NUM_TIMESTEPS; t++) {
        int sum = 0;
        for (int i = 0; i < NUM_PARTICLES; i++) {
            sum += particles[t][i];
        }
        estimated_states[t] = (double)sum / NUM_PARTICLES;
    }

    printf("True States: ");
    for (int t = 0; t < NUM_TIMESTEPS; t++) {
        printf("%d ", true_states[t]);
    }
    printf("\n");

    printf("Estimated States: ");
    for (int t = 0; t < NUM_TIMESTEPS; t++) {
        printf("%.2f ", estimated_states[t]);
    }
    printf("\n");

    return 0;
}