卡尔曼滤波（KF)和增广卡尔曼滤波（EKF）实现

本文主要是介绍卡尔曼滤波（KF)和增广卡尔曼滤波（EKF）实现，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

卡尔曼滤波（KF)

python实现：

import numpy as npF = np.array([[1, 1], [0, 1]])  # 状态转移矩阵 X(k+1)=[[1, Δt], [0, 1]]*X(k) Δt=1
Q = 0.1 * np.eye(2, 2)          # 过程噪声协方差矩阵
R = 0.1 * np.eye(2, 2)          # 观测噪声协方差矩阵            
H = np.eye(2, 2)                # 状态观测矩阵if __name__ == "__main__":X0 = np.array([[0], [1]])       # 初始位置与速度 X(k)=[X, X']X_true = np.array(X0)           # 真实状态初始化X_posterior = np.array(X0)      # 上一时刻的最优估计值P_posterior = np.eye(2, 2)      # 继续更新最优解的协方差矩阵for i in range(10):# 生成真实值 w = Q @ np.random.randn(2, 1)  # 生成过程噪声X_true = F @ X_true + w                                                                          # 得到当前时刻实际的速度值和位置值                                       # 生成观测值v = R @ np.random.randn(2, 1)  # 生成观测噪声 Z_measure = H @ X_true + v                                                                       # 生成观测值，H为单位阵# 进行先验估计X_prior = F @ X_posterior                                       # 计算状态估计协方差矩阵PP_prior = F @ P_posterior @ F.T + Q                  # 计算卡尔曼增益K = P_prior @ H.T @ np.linalg.inv(H @ P_prior @ H.T + R)         # 后验估计X_posterior = X_prior + K @ (Z_measure - H @ X_prior)                    # 更新状态估计协方差矩阵P     P_posterior = (np.eye(len(X_posterior)) - K @ H) @ P_prior  print(X_true.T, X_posterior.T)

C++实现：

#include <iostream>
#include <Eigen/Dense>int main(int argc, char* argv[])
{Eigen::Matrix2f F;F << 1, 1, 0, 1;Eigen::Matrix2f Q = 0.1 * Eigen::Matrix2f::Identity();Eigen::Matrix2f R = 0.1 * Eigen::Matrix2f::Identity();Eigen::Matrix2f H = Eigen::Matrix2f::Identity();Eigen::Vector2f X0;X0 << 0, 1;Eigen::Vector2f X_true = X0;Eigen::Vector2f X_posterior = X0;Eigen::Matrix2f P_posterior = Eigen::Matrix2f::Identity();for (size_t i = 0; i < 10; i++){Eigen::Vector2f w = Q * Eigen::Vector2f::Random();X_true = F * X_true + w;Eigen::Vector2f v = R * Eigen::Vector2f::Random();Eigen::Vector2f Z_measure = H * X_true + v;Eigen::Vector2f X_prior = F * X_posterior;Eigen::Matrix2f P_prior = F * P_posterior * F.transpose() + Q;Eigen::Matrix2f K = P_prior * H.transpose() * (H * P_prior * H.transpose() + R).inverse();X_posterior = X_prior + K * (Z_measure - H * X_prior);P_posterior = (Eigen::Matrix2f::Identity() - K * H) * P_prior;std::cout << "X_true: " << X_true.transpose() << " X_posterior: " << X_posterior.transpose() << std::endl;}return EXIT_SUCCESS;
}

增广卡尔曼滤波（EKF）

python实现：

import numpy as np
from math import sin, cosQ = 0.1 * np.eye(3, 3)
R = 0.1 * np.eye(2, 2)
H = np.array([[1, 0, 0], [0, 1, 0]]) def f(x, u):F = np.eye(3, 3)B = np.array([[0.1 * cos(x[2, 0]), 0], [0.1 * sin(x[2, 0]), 0], [0.0, 0.1]])x = F @ x + B @ u return xif __name__ == '__main__':X0 = np.zeros((3, 1))X_True = X0X_posterior = X0P_posterior = np.eye(3, 3)u = np.array([[10], [1]])for i in range(10):X_True = f(X_True, u)Z_measure = H @ X_True + R @ np.random.randn(2, 1)#  预测X_prior = f(X_posterior, u)v = u[0, 0]F = np.array([[1.0, 0.0, -0.1 * v * sin(X_posterior[2, 0])], [0.0, 1.0, 0.1 * v * cos(X_posterior[2, 0])], [0.0, 0.0, 1.0],])P_prior = F @ P_posterior @ F.T + Q # 预测方差#  更新K = P_prior @ H.T @ np.linalg.inv(H @ P_prior @ H.T + R ) # 卡尔曼增益X_posterior = X_prior + K @ (Z_measure - H @ X_prior) # 最优估计P_posterior = (np.eye(len(X_posterior)) - K @ H) @ P_prior # 最优估计方差print(X_True.T, X_posterior.T)

C++实现：

#include <iostream>
#include <Eigen/Dense>Eigen::VectorXf f(Eigen::VectorXf x, Eigen::VectorXf u)
{Eigen::MatrixXf F = Eigen::MatrixXf::Identity(3, 3);Eigen::MatrixXf B(3, 2);B << 0.1 * cos(x(2)), 0, 0.1* sin(x(2)), 0, 0, 0.1;x = F * x + B * u;return x;
}int main(int argc, char* argv[])
{Eigen::MatrixXf Q = 0.1 * Eigen::MatrixXf::Identity(3, 3);Eigen::MatrixXf R = 0.1 * Eigen::MatrixXf::Identity(2, 2);Eigen::MatrixXf H(2, 3);H << 1, 0, 0, 0, 1, 0;Eigen::VectorXf X0 = Eigen::VectorXf::Zero(3, 1);Eigen::VectorXf X_true = X0;Eigen::VectorXf X_posterior = X0;Eigen::MatrixXf P_posterior = Eigen::MatrixXf::Identity(3, 3);Eigen::VectorXf u(2, 1);u << 10, 1;for (size_t i = 0; i < 10; i++){X_true = f(X_true, u);Eigen::VectorXf Z_measure = H * X_true + R * Eigen::VectorXf::Random(2, 1);Eigen::VectorXf X_prior = f(X_posterior, u);float v = u(0);Eigen::MatrixXf F(3, 3);F << 1.0, 0.0, -0.1 * v * sin(X_posterior(2)), 0.0, 1.0, 0.1* v * cos(X_posterior(2)), 0.0, 0.0, 1.0;Eigen::MatrixXf P_prior = F * P_posterior * F.transpose() + Q;Eigen::MatrixXf K = P_prior * H.transpose() * (H * P_prior * H.transpose() + R).inverse();X_posterior = X_prior + K * (Z_measure - H * X_prior);P_posterior = (Eigen::MatrixXf::Identity(3, 3) - K * H) * P_prior;std::cout << "X_true: " << X_true.transpose() << " X_posterior: " << X_posterior.transpose() << std::endl;}return EXIT_SUCCESS;
}