卡尔曼滤波器公式

1、卡尔曼滤波公式如下

(1)预测方程:

预测状态向量=转换矩阵*上一时刻更新的状态向量 + 控制矩阵*当前的系统输入

/----------------------P推导 begin-----------------------------/

预测系统状态的协方差矩阵 = E(状态向量-预测状态向量)(状态向量-预测状态向量)转置

预测系统状态协方差矩阵 = 转换矩阵*上一时刻系统状态协方差矩阵*转换矩阵的转置+随机噪声*随机噪声转置

/----------------------P推导 end-----------------------------/

(2)更新方程:

/---------------------------------------辅助公式 begin-------------------------------------------------/

当前时刻测量值 = 测量矩阵*当前状态向量 + 测量噪声

测量值估计 = 测量矩阵*预测状态向量

/--------------------------------------辅助公式 end--------------------------------------------------/

状态向量估计 = 预测状态向量 + 卡尔曼增益(当前测量值-测量估计值)

卡尔曼增益 = 系统协方差矩阵*测量矩阵转置/(测量矩阵*系统协方差矩阵*测量矩阵转置)+高斯噪声

系统协方差矩阵 = 预测系统协方差矩阵 - 卡尔曼增益*测量矩阵*预测系统协方差矩阵

/-----------------P+推导 begin------------------------/

状态向量1:

状态向量2:

它们符合高斯分布,如下有,Rt为高斯噪声,

这两个状态向量的来源我们可以认为是独立的,因此他们的联合概率分布是各自概率分布的乘积。***重点是,高斯分布的乘积依旧是高斯分布!!!!***新的高斯分布的均值和方差有如下表达形式:

/-----------------P+推导 end----------------------/

相关推荐
电科_银尘7 小时前
【大模型算法与工程 15】手写Self-Attention:Q、K、V与缩放点积
算法
郝学胜-神的一滴10 小时前
Effective STL 条款03:容器中的对象复制要廉价且正确
开发语言·数据结构·c++·算法·rpc·stl
秋天的怀念_David10 小时前
算法一千零一夜 · 第三十三夜 · 跳表
java·程序人生·算法
Pniubi14 小时前
力扣21合并两个有序链表
算法·leetcode·职场和发展
AIgorithmGEEK14 小时前
std::unordered_map 与 std::unordered_set:接口、算法技巧与权衡 (速查)
c++·算法·哈希表·负载因子·桶·unordered_map·unordered_set
上海潭龙东海科技有限公司15 小时前
智慧堆场动态模型三维实时孪生重构智能管控
大数据·网络·数据库·算法·重构
阿庆_AI研发工程师17 小时前
Stanford CS336 Lecture 10:大语言模型推理与性能优化
算法
用户73416810354818 小时前
基于MindSpore的深度残差收缩网络程序
算法
Pniubi18 小时前
力扣239.滑动窗口最大值(优先队列)
算法·leetcode·职场和发展
青少儿编程课堂20 小时前
第十八届蓝桥杯青少组图形化真题 + 全国青少年信息素养大赛 + 电子学会图形化考级:校园泡泡堂大挑战解析
c++·python·算法·bfs·信息学竞赛