qr分解超定方程组matlab,matlab求解超定方程
function [sys，x0，str，ts]=Sr_UKF326(t，x，u，flag ) ]
globaln 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；
贝塔系数=2；
lamuda=alfa^2*(ngama )-n；
WM (1，1 )=lamuda/) nlamuda；
WM (2:2 * n1，1 )=0.5/) nlamuda；
Wc=Wm；
WC (1，1 )=lamuda/) nlamuda ) (1- alfa ^2beta )； %m是平均的权重系数，c是平均方差的权重系数
开关标志、
情况0，
[sys，x0，str，ts]=mdlInitializeSizes (；
情况2，
sys=MDLupdate(x，u，lamuda，sqQ，sqR，Wm，Wc )；
情况3，
sys=MDLoutputs(t，x，u )；
情况4，
sys=mdlgettimeofnextvarhit(t，x，u )；
case { 1，9 }，
sys=[]；
otherwise
da studio.error (Simulink : blocks : unhandled flag '，num2str ) flag )；
结束
function [sys，x0，str，ts]=mdlInitializeSizes (
global P0 S
global b%用于残差计算中的判定
全球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]；
functionsys=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振幅。 因为p保存着上次计算后留下的值
x (:1 )=xk1； %第一个点，4行1列的矩阵
squre_p=sqrt(nlamuda ) *S； %开根号
for i=1:n
X(X(:i1 ) ((x k1 squre _ p ) :I )； 求出从%2到n点的UT变换后的值
X(:In1(xk1-squre_p65:I )； 求出从%n1到2n点的UT变换后的值
结束
%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;