卡尔曼滤波MATLAB代码.docx
- 文档编号:2490193
- 上传时间:2022-10-30
- 格式:DOCX
- 页数:2
- 大小:15.72KB
卡尔曼滤波MATLAB代码.docx
《卡尔曼滤波MATLAB代码.docx》由会员分享,可在线阅读,更多相关《卡尔曼滤波MATLAB代码.docx(2页珍藏版)》请在冰豆网上搜索。
xt=Ftxt-1+Gtvtvt~N0,Qt
yt=Htxt+wtwt~N0,Rt
代码:
初始设定
%系数矩阵设定
F=[1,0.1;-1,0.9];G=[0,0.1];H=[1,0];
%系统噪音和观测噪音的设定
Q=1;R=1;
%数据个数
N=300;
%噪音生成
v=randn(N,1)*sqrtm(Q);
w=randn(N,1)*sqrtm(R);
%真值x和观测数据y的生成
x=zeros(N,2);y=zeros(N,1);
y
(1)=H*x(1,:
)'+w
(1);
fork=2:
N
x(k,:
)=F*x(k-1,:
)'+G'*v(k-1);
y(k)=H*x(k,:
)'+w(k);
end
%推测值的保存内存生成
xest=zeros(N,2);
%初始推测值设定
V=[1,0;0,1];xest(1,:
)=[0,0];
卡尔曼滤波主函数
function[xest_a,V_a,K]=KF(F,G,H,Q,R,y,xest,V)
%KF(F,G,H,Q,R,y,xest,V)
%x(t)=Fx(t-1)+Gv(t)
%y(t)=Hx(t)+w(t)
%y:
观测数据
%xest:
状态的初始推测值
%xest_a:
状态的推定値
%行向量变列向量
xest=xest(:
);
%预测阶段
xest_f=F*xest;%状态的预测值
V_f=F*V*F'+G*Q*G';
K=V_f*H'/(H*V_f*H'+R);
%推测阶段
xest_a=xest_f+K*(y-H*xest_f);%状态的推测值
V_a=V_f-K*H*V_f;
end
时刻2开始调用主函数
fork=2:
N
[xest(k,:
),V,K]=KF(F,G,H,Q,R,y(k),xest(k-1,:
),V);
end
输出图plot
figure
(1),clf
plot(1:
N,x(:
2),'black',1:
N,xest(:
2),'b--')
xlabel('时刻t'),ylabel('状态向量x的第二行'),legend('真值','状态的推测值')
有疑问群里找我
By刹那恍惚
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 卡尔 滤波 MATLAB 代码
![提示](https://static.bdocx.com/images/bang_tan.gif)