新闻详情

新闻详情

首页 / 资讯中心 / 详情

C++代码实现MATLAB中的kalman函数功能

发布时间:2026/11/17 22:03:02来源:尧图网络
C++代码实现MATLAB中的kalman函数功能
#includeiostream#includevector#includecmath#includeiomanip#includestdexcept#includealgorithm/* 极简矩阵类纯标准库 */classMatrix{public:introws,cols;std::vectordoubledata;Matrix():rows(0),cols(0){}Matrix(intr,intc):rows(r),cols(c),data(r*c,0.0){}Matrix(intr,intc,doublev):rows(r),cols(c),data(r*c,v){}doubleoperator()(inti,intj){returndata[i*colsj];}constdoubleoperator()(inti,intj)const{returndata[i*colsj];}staticMatrixidentity(intn){MatrixI(n,n);for(inti0;in;i)I(i,i)1.0;returnI;}Matrixtranspose()const{MatrixT(cols,rows);for(inti0;irows;i)for(intj0;jcols;j)T(j,i)(*this)(i,j);returnT;}Matrixoperator(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]o.data[i];returnR;}Matrixoperator-(constMatrixo)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]-o.data[i];returnR;}Matrixoperator*(constMatrixo)const{if(cols!o.rows)throwstd::runtime_error(Matrix dimension mismatch);MatrixR(rows,o.cols);for(inti0;irows;i)for(intk0;kcols;k){doublea(*this)(i,k);if(a0.0)continue;for(intj0;jo.cols;j)R(i,j)a*o(k,j);}returnR;}Matrixoperator*(doubles)const{MatrixR(rows,cols);for(size_t i0;idata.size();i)R.data[i]data[i]*s;returnR;}/* 高斯-约当消元法求逆带部分选主元 */Matrixinverse()const{if(rows!cols)throwstd::runtime_error(inverse: not square);intnrows;Matrix A*this;Matrix IMatrix::identity(n);for(inti0;in;i){intpivoti;for(intki1;kn;k)if(std::fabs(A(k,i))std::fabs(A(pivot,i)))pivotk;if(std::fabs(A(pivot,i))1e-12)throwstd::runtime_error(inverse: singular matrix);if(pivot!i)for(intj0;jn;j){std::swap(A(i,j),A(pivot,j));std::swap(I(i,j),I(pivot,j));}doubledA(i,i);for(intj0;jn;j){A(i,j)/d;I(i,j)/d;}for(intk0;kn;k){if(ki)continue;doublefA(k,i);if(f0.0)continue;for(intj0;jn;j){A(k,j)-f*A(i,j);I(k,j)-f*I(i,j);}}}returnI;}};/* 卡尔曼滤波器 */classKalmanFilter{public:Matrix A,B,C,Q,R,x,P,I;KalmanFilter(constMatrixA_,constMatrixB_,constMatrixC_,constMatrixQ_,constMatrixR_,constMatrixx0,constMatrixP0):A(A_),B(B_),C(C_),Q(Q_),R(R_),x(x0),P(P0){IMatrix::identity(A.rows);}/* 预测步等价于 MATLAB 的 predict * x A*x B*u * P A*P*A Q */Matrixpredict(constMatrixuMatrix()){xA*x;if(u.rows0B.cols0)xxB*u;PA*P*A.transpose()Q;returnx;}/* 校正步等价于 MATLAB 的 correct * K P*C/(C*P*CR) * x x K*(z - C*x) * P (I - K*C)*P */Matrixcorrect(constMatrixz){Matrix SC*P*C.transpose()R;// 新息协方差Matrix KP*C.transpose()*S.inverse();// 卡尔曼增益xxK*(z-C*x);// 更新状态P(I-K*C)*P;// 更新协方差returnx;}};/* 使用示例 */intmain(){constintn4;// 状态维数 [x, y, vx, vy]constintp2;// 测量维数 [x, y]constdoubledt0.1;// 采样周期// 状态转移矩阵匀速运动模型MatrixA(n,n);A(0,0)1;A(0,2)dt;A(1,1)1;A(1,3)dt;A(2,2)1;A(3,3)1;// 无控制输入B 的列数为 0MatrixB(n,0);// 观测矩阵仅观测位置MatrixC(p,n);C(0,0)1.0;C(1,1)1.0;// 过程噪声作用在速度分量上MatrixQ(n,n);Q(2,2)0.1;Q(3,3)0.1;// 测量噪声Matrix RMatrix::identity(p)*0.5;// 初始状态估计与协方差Matrixx0(n,1);x0(0,0)3.0;x0(1,0)3.0;x0(2,0)0.0;x0(3,0)0.0;Matrix P0Matrix::identity(n)*10.0;// 构造滤波器KalmanFilterkf(A,B,C,Q,R,x0,P0);std::coutstd::fixedstd::setprecision(6);// 在线滤波predict → correctfor(inti0;i100;i){// 构造模拟测量值真实轨迹x 10.5*t, y 20.3*tMatrixz(p,1);z(0,0)1.00.5*i*dt;z(1,0)2.00.3*i*dt;kf.predict();// 等价于 MATLAB: predict(kf)kf.correct(z);// 等价于 MATLAB: correct(kf, z)std::coutStep std::setw(3)i x [kf.x(0,0), kf.x(1,0), kf.x(2,0), kf.x(3,0)]\n;}return0;}
网站建设高端定制企业官网
RELATED

相关资讯

更多精彩内容,欢迎继续阅读

较早相关资讯

最新相关资讯

【外设】之大彩串口显示屏 2026/11/17 7:04:08

【外设】之大彩串口显示屏

大彩串口屏初步使用 1 .官网下载 STM32 屏幕 GUI 设计资料 http://www.gz-dc.com/category/typeid/4112 找到 STM32 Keil 工程,移植相关代码因项目而异进行移植,由于项目简单,本人只对用到的指令接口进行修改。 比如:注意事项&…

阅读更多 →
无法下载Windows系统iso文件 2026/11/17 5:42:44

无法下载Windows系统iso文件

当我遇到这个问题的时候,我打开了一个网站: 登录 然后我打算下载的时候: 突然那个官方的连接就可以下载了:

阅读更多 →
【清华代码熊】DeepSeek V4.1 Flash 后训练详解 2026/11/17 19:43:34

【清华代码熊】DeepSeek V4.1 Flash 后训练详解

📌 上期解析了 DeepSeek V4.1 Flash 模型架构改进,本期解析 DeepSeek V4.1 Flash 预训练/后训练技术: 🌟 预训练:45T 文本 多模态混合语料、直接训练 sparse attention(取消 DeepSeek V4 的 dense 冷启动&…

阅读更多 →
Shuffle-R1: Efficient RL framework for Multimodal Large Language Models via Data-centric Dynamic ... 2026/11/17 6:55:04

Shuffle-R1: Efficient RL framework for Multimodal Large Language Models via Data-centric Dynamic ...

文章主要内容和创新点 主要内容 本文聚焦于多模态大语言模型(MLLM)强化学习(RL)训练中的效率问题,提出了一个名为Shuffle-R1的框架。研究发现,当前RL训练存在两个关键缺陷: 优势值坍缩(Advantage Collapsing):批次中大多数优势值集中在零附近,导致有效梯度信号被淹…

阅读更多 →
PRvL: Quantifying the Capabilities and Risks of Large Language Models for PII Redaction 2026/11/17 5:57:47

PRvL: Quantifying the Capabilities and Risks of Large Language Models for PII Redaction

一、文章主要内容总结 本文聚焦于利用大型语言模型(LLMs)实现个人身份信息(PII)脱敏的研究,旨在解决传统脱敏方法(如基于规则的系统、领域特定命名实体识别(NER)模型)泛化能力差、跨格式/跨语境适应性弱的问题。 研究通过全面评估多种LLM架构(包括密集型LLM(D-LLM…

阅读更多 →
LLaVA-RE: Binary Image-Text Relevancy Evaluation with Multimodal Large Language Model 2026/11/17 19:43:32

LLaVA-RE: Binary Image-Text Relevancy Evaluation with Multimodal Large Language Model

文章主要内容和创新点 主要内容 本文聚焦于二进制图像-文本相关性评估任务(判断图像与文本“相关”或“不相关”),针对该任务中文本格式多样、相关性定义随场景变化等挑战,提出了基于多模态大语言模型(MLLM)的解决方案LLaVA-RE。 模型设计:LLaVA-RE基于LLaVA 1.5架构,…

阅读更多 →

今日资讯

本周资讯

本月资讯

看完文章仍有疑问?

联系尧图顾问,获取一对一建站咨询

立即免费咨询 📞 400-888-8888
📞 ✉