kalman1.m

MATLAB中用m文件实现卡尔曼滤波算法

function [sys,x0,str,ts] = kalman(t,x,u,flag)
switch flag,
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 2 
        sys=mdlUpdates(t,x,u);
    case 3
        sys=mdlOutputs(t,x,u);
    case {1,4,9}
        sys=[];
    otherwise
        error(['Unhandled flag =',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(FEstmtx,Hmtx,Cmtx);
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 4;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 2;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; 
sys = simsizes(sizes);
x0 =zeros(4,1);
str = [];
ts  = [1 0];
function sys=mdlUpdates(t,x,u,FEstmtx,Hmtx,Cmtx)
x(1)=Ia;x(2)=Ib;x(3)=W;x(4)=Theta
STEP=1;%步长值
koff=12000;%开始采样
ts=0.001;%仿真输入采样周期
tsest=STEP*ts;%估计采样周期
N=1000;
%MAXSAMPLES=floor(max(size(wr)-koff)/STEP):%最大采样值
Rs=1.26:Ls=6.5:Lamda=0.175;%电机定值设定
%PMTXINIT=0.1:%初始状态估算误差协方差值
%QMTXINIT=0.3;%过程噪声协方差阵
%RMTXINIT=0.2:%测量噪声协方差阵
%计算矩阵
U=zeros(2,1);Y=U;F=zeros(4,4):C=[1 0 0 0;0 1 0 0]
%Xestmtx=[0;0;0 ;0]
P(1,1)=0.1;P(2,2)=0.1;P(3,3)=200;Pmtx(4,4)=10
Q(1,1)=0.3;Q(1,1)=0.3;Q(3,3)=3:Q(4,4)=1;
R=[0.2 0;0 0.2];K=zeros(4,2)
Ia=zeros(N,1):Ib=Ia: W=Ia;
Theta=Ia;
%时间变量数值
al=1/Rs:a2=tsest/Ls:all=1+Rs*tsest*al;
F(l,l)=all:F(2,2)=all:F(3,3)=1;
F(l,2)=0:F(2,1)=0:F(3,1)=0;
F(3,2)=0:F(3,4)=0:F(4,1)=0;
F(4,2)=0:F(4,3)=tsest;
H=[a2 0;0 a2;0 0;0 0 ]
%开始参数估计循环
for k=1:N;
  sample=k*STEP+koff%采样时间
U(1)=Va(sample);U(2)=Vb(sample);Y(1)=Ia(sample);Y(2)=Ib(sample)
%先计算Fest矩阵(时间变化)
F(1,3)=-al*Lamda*tsest*sin(x(4));
F(1,4)=-al*Lamda*tsest*cos(x(4))*x(3);
F(2,3)=al*Lamda*tsest*cos(x(4));
F(2,4)=al*Lamda*tsest*sin(x(4))*x(3);
A=F;
%根据扩展卡尔曼滤波算法计算
K=F.*P.*Cmtx.*inv(C.*P.*C'+R);
x=A.*X+H.*U+K.*(Y-C.*x);
P=F.*P.*F+Q-K.*(C.*P.*C'+R).*K;
sys=F.*x+H.*U;
Y=C.*x;
%end
%保存变量
Ia(k+1)=x(1);
Ib(k+1)=x(2);
W(k+1)=x(3);
Theta(k+1)=x(4);
end
function sys=mdlOutputs(t,x,u,F,H,C)
sys=[x(3)
    x(4)];