卡尔曼滤波MATLAB代码Word格式.docx

卡尔曼滤波MATLAB代码Word格式.docx

《卡尔曼滤波MATLAB代码Word格式.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波MATLAB代码Word格式.docx（2页珍藏版）》请在冰豆网上搜索。

%系统噪音和观测噪音的设定

Q=1;

R=1;

%数据个数

N=300;

%噪音生成

v=randn（N,1）*sqrtm（Q）;

w=randn（N,1）*sqrtm（R）;

%真值ｘ和观测数据ｙ的生成

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;

时刻2开始调用主函数

[xest（k,:

）,V,K]=KF（F,G,H,Q,R,y（k）,xest（k-1,:

）,V）;

输出图plot

figure

（1）,clf

plot（1:

N,x（:

2）,'

black'

1:

N,xest（:

b--'

）

xlabel（'

时刻t'

）,ylabel（'

状态向量x的第二行'

）,legend（'

真值'

'

状态的推测值'

有疑问群里找我

By刹那恍惚