matlab qr无法使用,Matlab QR分解的一点疑问

function [sys,x0,str,ts] = SR_UKF326(t,x,u,flag)

global  n Rk Qk sqQ sqR Wm Wc P0

n = 4;%n表示系统的维数

Rk = [0.08,0;0,0.08];%量测噪声矩阵

Qk = [0.001,0,0,0;0,0.001,0,0;0,0,0.1,0;0,0,0,0.015];%观测噪声矩阵

P0 = [0.1,0,0,0;0,0.1,0,0;0,0,10,0;0,0,0,8];%协方差阵

sqR = chol(Rk);

sqQ = chol(Qk);

gama=sqrt(3);

alfa=0.01;

beta=2;

lamuda=alfa^2*(n+gama)-n;

Wm(1,1)=lamuda/(n+lamuda);

Wm(2:2*n+1,1)=0.5/(n+lamuda);

Wc=Wm;

Wc(1,1)=lamuda/(n+lamuda)+(1-alfa^2+beta);%m是均值的权系数，c是均方差的权系数

switch flag,

case 0,

[sys,x0,str,ts]=mdlInitializeSizes();

case 2,

sys=mdlUpdate(x,u,lamuda,sqQ,sqR,Wm,Wc);

case 3,

sys=mdlOutputs(t,x,u);

case 4,

sys = mdlGetTimeOfNextVarHit(t,x,u);

case { 1, 9 },

sys = [];

otherwise

DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end

function [sys,x0,str,ts]=mdlInitializeSizes()

global P0 S

global b%用在残差计算中判定

global Ck

sizes = simsizes;

sizes.NumContStates  = 0;

sizes.NumDiscStates  = 4;%状态变量分别为ialpha，ibeta，机械角速度，电角度

sizes.NumOutputs     = 4;

sizes.NumInputs      = 4;%输入变量分别为ialpha，ibeta，ualpha，ubeta

sizes.DirFeedthrough = 0;

sizes.NumSampleTimes = 1;

sys = simsizes(sizes);

x0  = [0;0;0;1.5*pi];%系统状态变量初始化

S = chol(P0);

x=x0;%这里进行状态变量x和协方差矩阵P的初始化

b=0;

Ck=0;

str = [];

ts  = [-1 0];

function sys=mdlUpdate(x,u,lamuda,sqQ,sqR,Wm,Wc)%每次进来只算一次，但一次有2n+1个点

global Rk Qk S n b Ck

t1 = 0.0001;%表示从上次计算到这次计算的时间

R = 2.875;

L = 8.5e-3;

PF = 0.175;

np = 4;%极对数

xk1=x;%将x幅值给xk1，因为x里面保存的是上次计算留下来的值

%如果是第一次，那么x里面就是保存着初始化程序中的x0

S=S;

%将S幅值给S，因为P里面保存的是上次计算留下来的值

X(:,1)=xk1;%第一个点，四行一列的矩阵

squre_P=sqrt(n+lamuda)*S; %开根号

for i=1:n

X(:,i+1)=xk1+squre_P(:,i);%求取2到n点的UT变换后的值

X(:,i+n+1)=xk1-squre_P(:,i);%求取n+1到2n点的UT变换后的值

end

%end UT

%时间更新

%状态预测值

[m,l]=size(X);%这行重点在于采样l，即变量X的列数，X的列数应该是始终等于2*n+1=9=l

r=zeros(4,l);%存储空间先放空,4行l列的存储空间

%*******将sigma点带入(离散化，但不是线性化的)状态转移方程得到结果*******%

%xk_k-1=f(xk-1,uk-1)%

for j=1:l

r(1,j)=(1-R*t1/L)*X(1,j)+PF*t1*np*X(3,j)*sin(X(4,j))/L+u(3)*t1/L+Qk(1,1);

r(2,j)=(1-R*t1/L)*X(2,j)-PF*t1*np*X(3,j)*cos(X(4,j))/L+u(4)*t1/L+Qk(2,2);

r(3,j)=X(3,j)+Qk(3,3);

r(4,j)=X(4,j)+np*X(3,j)*t1+Qk(4,4);

end

X_k_k_1=r;

%状态预测值均值

x_k_k_1=zeros(4,1);

%         x_k_k_1=X_k_k_1*Wm;%！！！！！！！！是不是漏了个求和

for i=1:2*n+1

x_k_k_1=x_k_k_1+Wm(i)*X_k_k_1(:,i);

end

[Temp S]=qr([sqrt(Wc(2))*(X_k_k_1(:,2:2*n+1)-x_k_k_1(:,ones(1,2*n))) sqQ]',0);%qr算出来的S是一个上三角矩阵我加了个0.05的衰减因子就可以得出比较接近正确结果的值

%         S=cholupdate(S,(X_k_k_1(:,1)-x_k_k_1(:)),'-');%这里cholupdate只更新i=0,上三角矩阵

S=chol(S'*S-(((-1*Wc(1))^(1/2))*(X_k_k_1(:,1)-x_k_k_1(:)))*(((-1*Wc(1))^(1/2))*(X_k_k_1(:,1)-x_k_k_1(:)))');

S=S';

%再次计算sigma点

Y(:,1)=x_k_k_1;%第一个点，四行一列的矩阵

squre_P=sqrt(n+lamuda)*S; %开根号

for i=1:n

Y(:,i+1)=x_k_k_1+squre_P(:,i);%求取2到n点的UT变换后的值

Y(:,i+n+1)=x_k_k_1-squre_P(:,i);%求取n+1到2n点的UT变换后的值

%X是4*7矩阵

end

%观测量预测值

o=zeros(2,2*n+1);

for i=1:2*n+1

o(1,i)=Y(1,i)+Rk(1,1);

o(2,i)=Y(2,i)+Rk(2,2);

end

Z_k_k_1=o;%Z_k_k_1是2*7矩阵

%观测量预测均值

z_k_k_1=zeros(2,1);

for i=1:2*n+1

z_k_k_1=z_k_k_1+Z_k_k_1(:,i)*Wm(i);%z_k_k_1是2*1矩阵

end

[Temp Sy]=qr([sqrt(Wc(2))*(Z_k_k_1(:,2:2*n+1)-z_k_k_1(:,ones(1,2*n))) sqR]',0);

%         Sy=cholupdate(Sy,(Z_k_k_1(:,1)-z_k_k_1(:)),'-');

Sy=chol(Sy'*Sy-(((-1*Wc(1))^(1/2))*(Z_k_k_1(:,1)-z_k_k_1(:)))*(((-1*Wc(1))^(1/2))*(Z_k_k_1(:,1)-z_k_k_1(:)))');

Sy=Sy';%Switch to lower triangular matrix下三角矩阵

%Calculate Kalman gain matrix

Pxy=zeros(4,2);

for i=1:2*n+1

Pxy=Pxy+(Y(:,i)-x_k_k_1(:,1))*Wc(i)*(Z_k_k_1(:,i)-z_k_k_1(:,1))';%4*2

end

cancha=[u(1);u(2)]-z_k_k_1;

if b==0%如果是第一次，那么Ck_1、Ck都赋值cancha*cancha'

Ck=cancha*cancha';

else%如果不是第一次了，那么，先把上一次的Ck放到Ck_1中去，然后再计算本次的#￥……#放入Ck

Ck=(0.95*Ck+cancha*cancha')/1.95;

end

MA=trace(Ck)/trace(Sy*Sy');

if MA>1

MA=MA;

else

MA=1;

end

K=((Pxy/MA)/(Sy)')/Sy;%4*2,所以Sy是一个2*(不知道是多少，运算给出的是2，但我觉得应该是9)的矩阵

%Update state estimate

x=x_k_k_1+K*cancha;

%Update state covariance

upMat=K*Sy;

for k=1:2

S=cholupdate(S,upMat(:,k),'-');%用chol算出来是一个上三角矩阵

end

S=S';%Switch to lower triangular matrix

b=b+1;

sys = x;

% end mdlDerivatives

%

%=============================================================================

% mdlOutputs

% Return the block outputs.

%=============================================================================

%

function sys=mdlOutputs(t,x,u)

sys = x;

% end mdlOutputs

function sys = mdlGetTimeOfNextVarHit(t,x,u)

sampleTime=0.0001;

sys=t+sampleTime;