卡尔曼滤波算法的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)

实验结果图:

相关推荐
地平线开发者2 小时前
让 AI Agent 和 征程6 台架高效、稳定、安全地协作
算法·安全·自动驾驶
ShineWinsu3 小时前
对于Qt:QGripLayout常见属性、方法以及QSizePolicy的解析
开发语言·c++·qt·面试·gui·客户端·网格
艾莉丝努力练剑3 小时前
【AI大模型接入SDK】C++的SessionManager 线程安全
java·开发语言·前端·c++·人工智能·学习·安全
kyle~3 小时前
SHA-256 哈希去重
算法·哈希算法
利威尔xu3 小时前
Go Mutex 饥饿模式:正常模式等待者饿死,1ms 阈值自动切换崩溃
开发语言·后端·面试·golang·并发编程
乌暮5 小时前
少写重复 CRUD:用大白话搞懂 MyBatis-Plus
java·开发语言·spring boot·面试·mybatis
不绝1915 小时前
Lua基础语法入门:注释、变量类型与字符串操作
开发语言·lua
Hhy_11076 小时前
《C++深度解构07》手撕 vector —— 从底层三指针架构到迭代器失效彻底吃透
c语言·开发语言·c++·学习·visual studio
风落青芜6 小时前
链式二叉树的实现与基本操作
c语言·开发语言·数据结构·算法
艾莉丝努力练剑6 小时前
【QT】系统相关:TCP的API介绍
开发语言·前端·c++·人工智能·qt·学习·tcp/ip