kalman.m

我这有个卡尔曼的mfile

function [sys, x0] = kalman(t,x,u,flag, T)
%kalman filter as an S-function (m-file)
global rr ll fai;
global out;
global x_1 P_1 K PP Q RR hh Y;
if flag == 0
rr=0.4;
ll=0.0086;
fai=0.213;
PP=[0.01,0,0,0;
    0,0.01,0,0;
    0,0,1000,0;
    0,0,0,2];
Q=[0.02,0,0,0;
    0,0.02,0,0;
    0,0,2000,0;
    0,0,0,1];
RR=[0.08,0;
      0,0.08];
x0 = zeros(4,1);
K=zeros(4,2);
sys = [0, 4, 4, 5, 0, 0];
elseif flag == 2
% calc. inp and out vec of kalman filter
U = [u(1); u(2)];
Y = [u(3); u(4)];
%prediction
diff_FI=[1-rr/ll*T, 0, fai/ll*sin(x(4))*T, x(3)*fai/ll*cos(x(4))*T;
           0, 1-rr/ll*T,-fai/ll*cos(x(4))*T, x(3)*fai/ll*sin(x(4))*T;
           0, 0,1, 0;
           0, 0,T, 1];
x_1=[diff_FI(1,1)*x(1)+diff_FI(1,3)*x(3);
diff_FI(2,2)*x(2)+diff_FI(2,3)*x(3);
diff_FI(3,3)*x(3);
diff_FI(4,3)*x(3)+diff_FI(4,4)*x(4)]+T*[u(1)/ll; u(2)/ll; 0; 0];
P_1=diff_FI*PP*diff_FI' + Q; %PP[k|k-1] is ready
% calculation of hh, diff_hh
hh=[x_1(1,1); x_1(2,1)];
diff_hh=[1,0,0,0;
         0,1,0,0];
%Sensorless Control with Kalman Filter on TMS320 Fixed-Point DSP 33
% system of Kalman filter
K=P_1*diff_hh'*inv(diff_hh*P_1*diff_hh' +RR); %K[k]
out = x_1+K*(Y-hh); %x[k] is ready
sys = out;
PP=P_1-K*diff_hh*P_1; %PP[k] is ready
elseif flag == 3
sys = out;
%elseif flag == 4
%sys = (round(t/T)+1)*T;
else
sys =[];
end