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

卡尔曼滤波（KF)

python实现：

import numpy as np


F = 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                                       

        # 计算状态估计协方差矩阵P
        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     
        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, cos
 
 
Q = 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 x


if __name__ == '__main__':
    X0 = np.zeros((3, 1))
    X_True = X0
    X_posterior = X0
    P_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;
}

参考：
【硬核总结】从基础卡尔曼滤波到互补卡尔曼滤波
扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）
常见滤波汇总（KF、EKF、UKF和PF）