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

2024-01-01 17:52
文章标签 实现 卡尔曼滤波 ekf 增广 kf

本文主要是介绍卡尔曼滤波（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;
}

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

这篇关于卡尔曼滤波（KF)和增广卡尔曼滤波（EKF）实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/560050。

相关文章

C++对象布局及多态实现探索之内存布局（整理的很多链接）

C++对象布局及多态实现探索之内存布局（整理的很多链接）

本文通过观察对象的内存布局，跟踪函数调用的汇编代码。分析了C++对象内存的布局情况，虚函数的执行方式，以及虚继承，等等 文章链接：http://dev.yesky.com/254/2191254.shtml      论C/C++函数间动态内存的传递 (2005-07-30)   当你涉及到C/C++的核心编程的时候，你会无止境地与内存管理打交道。 文章链接：http://dev.yesky
阅读更多...
通过SSH隧道实现通过远程服务器上外网

通过SSH隧道实现通过远程服务器上外网

搭建隧道 autossh -M 0 -f -D 1080 -C -N user1@remotehost##验证隧道是否生效，查看1080端口是否启动netstat -tuln | grep 1080## 测试ssh 隧道是否生效curl -x socks5h://127.0.0.1:1080 -I http://www.github.com 将autossh 设置为服务，隧道开机启动
阅读更多...
时序预测 | MATLAB实现LSTM时间序列未来多步预测-递归预测

时序预测 | MATLAB实现LSTM时间序列未来多步预测-递归预测

时序预测 | MATLAB实现LSTM时间序列未来多步预测-递归预测 目录 时序预测 | MATLAB实现LSTM时间序列未来多步预测-递归预测基本介绍程序设计参考资料 基本介绍 MATLAB实现LSTM时间序列未来多步预测-递归预测。LSTM是一种含有LSTM区块（blocks）或其他的一种类神经网络，文献或其他资料中LSTM区块可能被描述成智能网络单元，因为
阅读更多...
vue项目集成CanvasEditor实现Word在线编辑器

vue项目集成CanvasEditor实现Word在线编辑器

CanvasEditor实现Word在线编辑器 官网文档：https://hufe.club/canvas-editor-docs/guide/schema.html 源码地址：https://github.com/Hufe921/canvas-editor 前提声明： 由于CanvasEditor目前不支持vue、react 等框架开箱即用版，所以需要我们去Git下载源码，拿到其中两个主
阅读更多...
android一键分享功能部分实现

android一键分享功能部分实现

为什么叫做部分实现呢，其实是我只实现一部分的分享。如新浪微博，那还有没去实现的是微信分享。还有一部分奇怪的问题：我QQ分享跟QQ空间的分享功能，我都没配置key那些都是原本集成就有的key也可以实现分享，谁清楚的麻烦详解下。 实现分享功能我们可以去www.mob.com这个网站集成。免费的，而且还有短信验证功能。等这分享研究完后就研究下短信验证功能。 开始实现步骤（新浪分享，以下是本人自己实现
阅读更多...
基于Springboot + vue 的抗疫物质管理系统的设计与实现

基于Springboot + vue 的抗疫物质管理系统的设计与实现

目录 📚 前言 📑摘要 📑系统流程 📚 系统架构设计 📚 数据库设计 📚 系统功能的具体实现    💬 系统登录注册 系统登录 登录界面   用户添加  💬 抗疫列表展示模块     区域信息管理 添加物资详情 抗疫物资列表展示 抗疫物资申请 抗疫物资审核 ✒️ 源码实现 💖 源码获取 😁 联系方式 📚 前言 📑博客主页：
阅读更多...
探索蓝牙协议的奥秘：用ESP32实现高质量蓝牙音频传输

探索蓝牙协议的奥秘：用ESP32实现高质量蓝牙音频传输

蓝牙（Bluetooth）是一种短距离无线通信技术，广泛应用于各种电子设备之间的数据传输。自1994年由爱立信公司首次提出以来，蓝牙技术已经经历了多个版本的更新和改进。本文将详细介绍蓝牙协议，并通过一个具体的项目——使用ESP32实现蓝牙音频传输，来展示蓝牙协议的实际应用及其优点。 蓝牙协议概述 蓝牙协议栈 蓝牙协议栈是蓝牙技术的核心，定义了蓝牙设备之间如何进行通信。蓝牙协议
阅读更多...
python实现最简单循环神经网络(RNNs)

python实现最简单循环神经网络(RNNs)

Recurrent Neural Networks（RNNs） 的模型： 上图中红色部分是输入向量。文本、单词、数据都是输入，在网络里都以向量的形式进行表示。 绿色部分是隐藏向量。是加工处理过程。 蓝色部分是输出向量。 python代码表示如下： rnn = RNN()y = rnn.step(x) # x为输入向量，y为输出向量 RNNs神经网络由神经元组成， python
阅读更多...
利用Frp实现内网穿透（docker实现）

利用Frp实现内网穿透（docker实现）

文章目录 1、WSL子系统配置2、腾讯云服务器安装frps2.1、创建配置文件2.2 、创建frps容器 3、WSL2子系统Centos服务器安装frpc服务3.1、安装docker3.2、创建配置文件3.3 、创建frpc容器 4、WSL2子系统Centos服务器安装nginx服务 环境配置：一台公网服务器（腾讯云）、一台笔记本电脑、WSL子系统涉及知识：docker、Frp
阅读更多...
基于 Java 实现的智能客服聊天工具模拟场景

基于 Java 实现的智能客服聊天工具模拟场景

服务端代码 import java.io.BufferedReader;import java.io.IOException;import java.io.InputStreamReader;import java.io.PrintWriter;import java.net.ServerSocket;import java.net.Socket;public class Serv
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2