![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% 状态
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% xk=A•xk-1+B•uk+wk
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% zk=H•xk+vk,
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% p(w) ~ N(0,Q)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% p(v) ~ N(0,R),
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% 预测
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% x'k=A•xk+B•uk
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% P'k=A•P(k-1)*AT + Q
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% 修正
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% Kk=P'k•HT•(H•P'k•HT+R)-1
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% xk=x'k+Kk•(zk-H•x'k)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
% Pk=(I-Kk•H)•P'k
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
%要注意的是:必须把系统状态和kalman滤波器内部预测的状态分开
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
function Test
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
A=[1 0.1;0 1];
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
B=0;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Xp=rand(2,1)*0.1;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
X=[0 0]';
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
H=[1 0];
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Q=eye(2)*1e-5;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
R=eye(1)*0.1;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
P=eye(2);% P'(k)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
angle=[];
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
angle_m=[];
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
angle_real=[];
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
for i=1:500
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
angle_real=[angle_real X(1)]; %实际角度
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
[Xp,P]=Predict(A,Xp,P,Q);
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
X=A*X+rand(2,1)*1e-5;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
z_m=H*X+rand(1,1)*0.1-0.05;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
angle_m=[angle_m z_m(1)]; %测量的角度
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
[Xp,P]=Correct(P,H,R,X,z_m);
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
angle=[angle Xp(1)]; %预测的角度
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
end
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
t=1:500;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
plot(t,angle,'r',t,angle_m,'g',t,angle_real,'b')
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
legend('预测值','测量值','实际值')
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
figure
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
plot(t,angle-angle_real,'r',t,angle_m-angle_real,'g')
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
legend('滤波后的误差','测量的误差')
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
title('误差分析')
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
xlabel('time');
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
ylabel('error');
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
function [Xk,Pk]=Predict(A,Xk,Pk_1,Q)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Xk=A*Xk;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Pk=A*Pk_1*A'+Q;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
function [Xk,Pk]=Correct(Pk,H,R,Xk,zk)
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Kk=Pk * H' * inv(H * Pk * H' + R);
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Xk=Xk+ Kk*(zk-H*Xk);
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)
Pk=(eye(size(Pk,1)) - Kk*H)*Pk;
![](http://www.cnblogs.com/Images/OutliningIndicators/None.gif)