粒子滤波（Particle Filter），也被称为序列蒙特卡罗方法，是一种用于非线性系统状态估计的贝叶斯滤波方法。以下是一个简单的粒子滤波算法的C语言代码示例。这段代码展示了如何实现一个基本的粒子滤波器，包括初始化粒子、更新粒子权重、重采样等步骤。

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

#define NUM_PARTICLES 100
#define STATE_DIM 2
#define SENSOR_NOISE 0.1
#define MOTION_NOISE 0.1

typedef struct {
    double x;
    double y;
    double weight;
} Particle;

void initialize_particles(Particle particles[], int num_particles);
void predict_particles(Particle particles[], int num_particles, double delta_x, double delta_y);
void update_weights(Particle particles[], int num_particles, double measured_x, double measured_y);
void resample_particles(Particle particles[], int num_particles, Particle new_particles[]);

int main() {
    Particle particles[NUM_PARTICLES];
    Particle new_particles[NUM_PARTICLES];
    
    double measured_x = 1.0;
    double measured_y = 1.0;
    double delta_x = 0.5;
    double delta_y = 0.5;

    // Initialize particles
    initialize_particles(particles, NUM_PARTICLES);
    
    // Predict particles
    predict_particles(particles, NUM_PARTICLES, delta_x, delta_y);
    
    // Update particle weights
    update_weights(particles, NUM_PARTICLES, measured_x, measured_y);
    
    // Resample particles
    resample_particles(particles, NUM_PARTICLES, new_particles);

    // Output resampled particles
    for (int i = 0; i < NUM_PARTICLES; i++) {
        printf("Particle %d: x = %.2f, y = %.2f, weight = %.2f\n",
               i, new_particles[i].x, new_particles[i].y, new_particles[i].weight);
    }
    
    return 0;
}

void initialize_particles(Particle particles[], int num_particles) {
    for (int i = 0; i < num_particles; i++) {
        particles[i].x = (double)rand() / RAND_MAX * 10.0; // Random position in range [0, 10)
        particles[i].y = (double)rand() / RAND_MAX * 10.0;
        particles[i].weight = 1.0 / num_particles; // Initial weight
    }
}

void predict_particles(Particle particles[], int num_particles, double delta_x, double delta_y) {
    for (int i = 0; i < num_particles; i++) {
        particles[i].x += delta_x + ((double)rand() / RAND_MAX - 0.5) * MOTION_NOISE;
        particles[i].y += delta_y + ((double)rand() / RAND_MAX - 0.5) * MOTION_NOISE;
    }
}

void update_weights(Particle particles[], int num_particles, double measured_x, double measured_y) {
    double total_weight = 0.0;
    for (int i = 0; i < num_particles; i++) {
        double dx = particles[i].x - measured_x;
        double dy = particles[i].y - measured_y;
        particles[i].weight = exp(-0.5 * (dx * dx + dy * dy) / (SENSOR_NOISE * SENSOR_NOISE));
        total_weight += particles[i].weight;
    }
    
    // Normalize weights
    for (int i = 0; i < num_particles; i++) {
        particles[i].weight /= total_weight;
    }
}

void resample_particles(Particle particles[], int num_particles, Particle new_particles[]) {
    double *cumulative_weights = (double *)malloc(num_particles * sizeof(double));
    cumulative_weights[0] = particles[0].weight;
    for (int i = 1; i < num_particles; i++) {
        cumulative_weights[i] = cumulative_weights[i - 1] + particles[i].weight;
    }
    
    for (int i = 0; i < num_particles; i++) {
        double r = (double)rand() / RAND_MAX;
        double target = r * cumulative_weights[num_particles - 1];
        
        int low = 0;
        int high = num_particles - 1;
        while (low < high) {
            int mid = (low + high) / 2;
            if (cumulative_weights[mid] < target) {
                low = mid + 1;
            } else {
                high = mid;
            }
        }
        
        new_particles[i] = particles[low];
    }
    
    free(cumulative_weights);
}

代码说明：

1. 初始化粒子：每个粒子的位置在 [0, 10) 范围内随机生成，权重初始化为 1 / num_particles。
2. 预测粒子：根据运动模型预测粒子的位置，这里使用了简单的加权噪声来模拟实际的运动。
3. 更新权重：根据观测值和粒子位置更新权重，使用高斯分布来计算粒子与观测值的匹配程度。
4. 重采样粒子：根据更新后的权重对粒子进行重采样，确保高权重的粒子有更高的概率被选中。

请根据实际需求对代码进行调整和优化，特别是在粒子数量、运动模型和观测模型方面。