卡尔曼滤波算法的matlab实现

卡尔曼滤波算法的matlab实现

matlab 复制代码
figure;
hold on;

Z=(1:1:100);  %观测值:第一秒观测1m 第二秒观测两米  匀速运动, 每秒1m,  最后拟合的也是速度 1m/s

plot(Z);
plot([0,100], [1,1]);

noise=randn(1,100)*0.5;  %生成方差为1的高斯噪声
Z=Z+noise;  % 加入噪声

plot(Z);

X=[0;0];  %初始状态 随意设置; 经过几次迭代后,接近真实的状态
P=[1 0;0 1];  %状态协方差矩阵 :随意设置;   经过几次迭代后,接近真实的状态
F=[1 1;0 1];  %状态转移矩阵, \Delta{t} 为 1, 每 秒采样一次
Q=[0.0001, 0;0 0.0001];  % 状态转移协方差矩阵:因为相信状态转移矩阵不会出错, 因此 Q方差 很小
H=[1 0];  % 观测矩阵
R=cov(noise);  % 观测矩阵协方差, 与加入的方差为1的高斯噪声相对应;

for i=1:100
    X_=F*X;  % 预测状态公式, 代码中缺乏控制量
    P_=F*P*F'+Q;
    K=P_*H'/(H*P_*H'+R);
    X=X_+K*(Z(i)-H*X_);
    P=(eye(2)-K*H)*P_;  % eye(2)是二阶单位阵
    
    % plot(X(1),X(2),'x');
    scatter(Z(i), X(1),'.');  % 画点, 横轴表示位置, 纵轴表示速度
    scatter(Z(i), X(2),'.');  % 画点, 横轴表示位置, 纵轴表示速度
    
end

%plot(Z, Z)

实验结果图:

相关推荐
程序员JerrySUN2 小时前
驱动开发硬核特训 · Day 22(下篇): # 深入理解 Power-domain 框架:概念、功能与完整代码剖析
linux·开发语言·驱动开发·嵌入式硬件
椰羊~王小美2 小时前
LeetCode -- Flora -- edit 2025-04-27
算法·leetcode·职场和发展
游离状态的猫12 小时前
JavaScript性能优化实战:从瓶颈定位到极致提速
开发语言·javascript·性能优化
GeekABC2 小时前
FastAPI系列06:FastAPI响应(Response)
开发语言·python·fastapi·web
小彭努力中2 小时前
7.Three.js 中 CubeCamera详解与实战示例
开发语言·前端·javascript·vue.js·ecmascript
why1513 小时前
腾讯(QQ浏览器)后端开发
开发语言·后端·golang
charade3123 小时前
【C语言】内存分配的理解
c语言·开发语言·c++
LinDaiuuj3 小时前
判断符号??,?. ,! ,!! ,|| ,&&,?: 意思以及举例
开发语言·前端·javascript
缘友一世3 小时前
从线性回归到逻辑回归
算法·逻辑回归·线性回归
小臭希4 小时前
Java——琐碎知识点一
java·开发语言