close all
clear all
%讀取數據，從中取出電流電壓等值
load shuju.mat
t1=shuju(1,:);
Ua=shuju(2,:);
Ub=shuju(3,:);
Ia=shuju(4,:);
Ib=shuju(5,:);
v=shuju(6,:);
s=shuju(7,:);
%電機定值設定
Rs=2.65;L=0.0267;Ke=59.5;tao=0.016;
ts=t1(2)-t1(1)
Maxk=floor(max(size(t1)))
B=[1/L,0;0,1/L;0,0;0,0];
H=[1,0,0,0;0,1,0,0];
Q=[0.3,0,0,0;0,0.3,0,0;0,0,0.3,0;0,0,0,3];
R=[2e-5,0;0,2e-5];
XX=zeros(4,1);
PP=[1e-6,0,0,0;0,1e-6,0,0;0,0,1e-6,0;0,0,0,2.5];
U=zeros(2,1);Y=zeros(2,1);
IaE=zeros(Maxk,1);IbE=IaE;vE=IaE;sE=IaE;

%計算矩陣
F=zeros(4,4);
f=zeros(4,1);
a1=-Rs/L;a2=Ke/L;a3=pi/tao
F(1,1)=a1;F(1,2)=0;
F(2,1)=0;F(2,2)=a1;
F(3,1)=0;F(3,2)=0;F(3,3)=0;F(3,4)=0;
F(4,1)=0;F(4,2)=0;F(4,3)=1;F(4,4)=0;
%開始循環計算
for k=1:Maxk
    U(1)=Ua(k);U(2)=Ub(k);Y(1)=Ia(k);Y(2)=Ib(k);
    f(1)=a1*XX(1)+a2*XX(3)*sin(a3*XX(4));
    f(2)=a1*XX(2)+a2*XX(3)*cos(a3*XX(4));
    f(3)=0;
    f(4)=XX(3);
    F(1,3)=a2*sin(a3*XX(4));
    F(1,4)=a2*a3*cos(a3*XX(4));
    F(2,3)=-a2*cos(a3*XX(4));
    F(2,4)=a2*a3*sin(a3*XX(4));
%預測
    X=XX+f*ts+B*U*ts;
    P=PP+F*PP*ts+PP*F'*ts+Q;
%更新
    K=P*H'*inv(H*P*H'+R);
    XX=X+K*Y-K*H*X;
    PP=P-K*H*P;
%保存變量
IaE(k)=XX(1);
IbE(k)=XX(2);
vE(k)=XX(3);
sE(k)=XX(4);
%計算均方根誤差

%計算穩態誤差
end