写一个粒子滤波的c++代码

以下是一个简单的粒子滤波器的C++代码示例：

#include <iostream>
#include <random>
#include <vector>

using namespace std;

// 定义粒子
struct Particle {
    double x; // 状态变量
    double weight; // 权重
};

// 粒子滤波器类
class ParticleFilter {
public:
    // 构造函数，需要指定粒子数和状态变量维度
    ParticleFilter(int numParticles, int stateDim) {
        numParticles_ = numParticles;
        stateDim_ = stateDim;
        particles_.resize(numParticles_);
        weights_.resize(numParticles_);
    }

    // 初始化粒子状态和权重
    void initParticles(double* initState) {
        default_random_engine generator;
        normal_distribution<double> distribution(0.0, 1.0);

        for (int i = 0; i < numParticles_; ++i) {
            Particle& particle = particles_[i];
            particle.x = initState[i % stateDim_] + distribution(generator);
            particle.weight = 1.0 / numParticles_;
        }
    }

    // 预测步骤
    void predict(double* control, double* motionModel) {
        default_random_engine generator;
        normal_distribution<double> distribution(0.0, 1.0);

        for (int i = 0; i < numParticles_; ++i) {
            Particle& particle = particles_[i];
            for (int j = 0; j < stateDim_; ++j) {
                particle.x += motionModel[j] * control[j] + distribution(generator);
            }
        }
    }

    // 更新步骤
    void update(double* measurement, double* measurementModel) {
        double sumWeights = 0.0;
        for (int i = 0; i < numParticles_; ++i) {
            Particle& particle = particles_[i];
            double innov = measurement[0] - measurementModel[0] * particle.x;
            particle.weight = exp(-0.5 * innov * innov / 0.1);
            sumWeights += particle.weight;
        }

        // 归一化权重
        for (int i = 0; i < numParticles_; ++i) {
            particles_[i].weight /= sumWeights;
        }
    }

    // 重采样步骤
    void resample() {
        vector<Particle> newParticles(numParticles_);
        default_random_engine generator;
        discrete_distribution<int> distribution(weights_.begin(), weights_.end());

        for (int i = 0; i < numParticles_; ++i) {
            int index = distribution(generator);
            newParticles[i].x = particles_[index].x;
            newParticles[i].weight = 1.0 / numParticles_;
        }

        particles_ = newParticles;
    }

    // 获取粒子均值
    double getMean() const {
        double mean = 0.0;
        for (int i = 0; i < numParticles_; ++i) {
            mean += particles_[i].x * particles_[i].weight;
        }
        return mean;
    }

private:
    int numParticles_; // 粒子数
    int stateDim_; // 状态变量维度
    vector<Particle> particles_; // 粒子
    vector<double> weights_; // 权重
};

int main() {
    // 创建粒子滤波器
    ParticleFilter pf(1000, 1);

    // 初始化粒子
    double initState[1] = {0.0};
    pf.initParticles(initState);

    // 进行预测、更新和重采样步骤
    double control[1] = {1.0};
    double motionModel[1] = {1.0};
    double measurement[1] = {2.0};
    double measurementModel[1] = {1.0};
    pf.predict(control, motionModel);
    pf.update(measurement, measurementModel);
    pf.resample();

    // 输出粒子均值
    cout << "Mean: " << pf.getMean() << endl;

    return 0;
}

这是一个非常简单的粒子滤波器，仅包含一维状态变量和一维测量值。在实际应用中，需要根据具体问题进行修改和扩展。