卡尔曼滤波算法C++代码
时间: 2023-10-12 17:01:27 浏览: 82
以下是一个简单的卡尔曼滤波算法的C++代码示例:
```cpp
#include <iostream>
#include <vector>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
// 卡尔曼滤波类
class KalmanFilter {
public:
KalmanFilter(const VectorXd& x0, const MatrixXd& P0, const MatrixXd& A,
const MatrixXd& Q, const MatrixXd& H, const MatrixXd& R)
: x(x0), P(P0), A(A), Q(Q), H(H), R(R) {}
void predict() {
x = A * x;
P = A * P * A.transpose() + Q;
}
void update(const VectorXd& z) {
MatrixXd S = H * P * H.transpose() + R;
MatrixXd K = P * H.transpose() * S.inverse();
VectorXd y = z - H * x;
x = x + K * y;
P = (MatrixXd::Identity(P.rows(), P.cols()) - K * H) * P;
}
VectorXd getState() const { return x; }
private:
VectorXd x; // 状态向量
MatrixXd P; // 状态协方差矩阵
MatrixXd A; // 状态转移矩阵
MatrixXd Q; // 过程噪声协方差矩阵
MatrixXd H; // 观测矩阵
MatrixXd R; // 观测噪声协方差矩阵
};
int main() {
// 初始化卡尔曼滤波器参数
VectorXd x0(2); // 初始状态向量
x0 << 0, 0;
MatrixXd P0(2, 2); // 初始状态协方差矩阵
P0 << 1, 0, 0, 1;
MatrixXd A(2, 2); // 状态转移矩阵
A << 1, 1, 0, 1;
MatrixXd Q(2, 2); // 过程噪声协方差矩阵
Q << 0.1, 0, 0, 0.1;
MatrixXd H(1, 2); // 观测矩阵
H << 1, 0;
MatrixXd R(1, 1); // 观测噪声协方差矩阵
R << 1;
// 构造卡尔曼滤波器
KalmanFilter kf(x0, P0, A, Q, H, R);
// 模拟观测数据
vector<double> measurements = {1.1, 2.0, 3.5, 4.9};
vector<double> filteredResults;
// 运行卡尔曼滤波器
for (double z : measurements) {
kf.predict();
kf.update(VectorXd(1) << z);
filteredResults.push_back(kf.getState()(0));
}
// 打印滤波结果
cout << "滤波结果:" << endl;
for (double result : filteredResults) {
cout << result << endl;
}
return 0;
}
```
这段代码实现了一个简单