利用matlab实现最小二乘估计
最小二乘估计概念
古典最小二乘估计
tic
clc;
clear;
%首先假定量测量值如下
mul=[100,200,300];
sigma=[4,0,0;0,4,0;0,0,16];
data=mvnrnd(mul,sigma,100);
%假定量测量Z是按照100个x，y,z交替排列
Z=zeros(300,1);
for i=1:100
Z(3*i-2,1)=data(i,1);
Z(3*i-1,1)=data(i,2);
Z(3*i,1)=data(i,3);
end
%定义H矩阵300*3
H=zeros(300,3);
for i=1:100
H(3*i-2,1)=1;
H(3*i-1,2)=1;
H(3*i,3)=1;
end
%方差矩阵300*300
R=zeros(300,300);
for i=1:100
R(3*i-2,3*i-2)=4;
R(3*i-1,3*i-1)=4;
R(3*i,3*i)=16;
end
%古典最小二乘法估计
X=inv(H'*H)*H'*Z
E=inv(H'*H)*H'*R*H*inv(H'*H)
toc
输出结果：
X =
99.7247
200.2023
300.1888
E =
0.0400 0 0
0 0.0400 0
0 0 0.1600
时间已过 0.007059 秒。
加权最小二乘估计
tic
clc;
clear;
%首先假定量测量值如下
mul=[100,200,300];
sigma=[4,0,0;0,4,0;0,0,16];
data=mvnrnd(mul,sigma,100);
%假定量测量Z是按照100个x，y,z交替排列
Z=zeros(300,1);
for i=1:100
Z(3*i-2,1)=data(i,1);
Z(3*i-1,1)=data(i,2);
Z(3*i,1)=data(i,3);
end
%定义H矩阵300*3
H=zeros(300,3);
for i=1:100
H(3*i-2,1)=1;
H(3*i-1,2)=1;
H(3*i,3)=1;
end
%方差矩阵300*300
R=zeros(300,300);
for i=1:100
R(3*i-2,3*i-2)=4;
R(3*i-1,3*i-1)=4;
R(3*i,3*i)=16;
end
W=inv(R);
%加权最小二乘法估计
X=inv(H'*W*H)*H'*W*Z
E=inv(H'*R'*H)
toc
输出结果：
X =
100.1899
200.1390
300.0483
E =
0.0025 0 0
0 0.0025 0
0 0 0.0006
时间已过 0.123054 秒。
递推最小二乘估计
tic
clc;
clear;
%首先假定量测量值如下
mul=[100,200,300];
sigma=[4,0,0;0,4,0;0,0,16];
data=mvnrnd(mul,sigma,100);
%假定量测量Z是按照100个x，y,z交替排列
Z=zeros(300,1);
for i=1:100
Z(3*i-2,1)=data(i,1);
Z(3*i-1,1)=data(i,2);
Z(3*i,1)=data(i,3);
end
%递推最小二乘法估计
%假定上一时刻的X，P
X=[0;0;0];
P=[3,0,0;0,5,0;0,0,17];
%定义H，R
H=eye(3);
R=[4,0,0;0,4,0;0,0,16];
for i=1:100
Zk=[Z(3*i-2,1);Z(3*i-1,1);Z(3*i,1)];
K=P*H'*inv(H*P*H'+R);
X=X+K*(Zk-H*X);
P=P-K*H*P;
end
X
P
toc
X =
98.7186
198.2282
297.0573
P =
0.0395 0 0
0 0.0397 0
0 0 0.1585
时间已过 0.009759 秒。
tic
clc;
clear;
%首先假定量测量值如下
mul=[100,200,300];
sigma1=[4,0,0;0,4,0;0,0,16];
sigma2=[1,0,0;0,1,0;0,0,1];
data1=mvnrnd(mul,sigma1,100);
data2=mvnrnd(mul,sigma2,100);
%假定量测量Z是按照100个x1,y1,z1,x2,y2,z2交替排列
Z=zeros(600,1);
for i=1:100
Z(6*i-5,1)=data1(i,1);
Z(6*i-4,1)=data1(i,2);
Z(6*i-3,1)=data1(i,3);
Z(6*i-2,1)=data2(i,1);
Z(6*i-1,1)=data2(i,2);
Z(6*i,1)=data2(i,3);
end
%递推最小二乘法估计
%假定上一时刻的X，P
X=[0;0;0];
P=[1,0,0;0,1,0;0,0,1];
%定义H，R
H=[1,0,0;
0,1,0;
0,0,1;
1,0,0;
0,1,0;
0,0,1];
R=[4,0,0,0,0,0;
0,4,0,0,0,0;
0,0,16,0,0,0;
0,0,0,1,0,0;
0,0,0,0,1,0;
0,0,0,0,0,1];
for i=1:100
Zk=[Z(6*i-5,1);Z(6*i-4,1);Z(6*i-3,1);Z(6*i-2,1);Z(6*i-1,1);Z(6*i,1)];
K=P*H'*inv(H*P*H'+R);
X=X+K*(Zk-H*X);
P=P-K*H*P;
end
X
P
toc
输出结果：
X =
99.1077
198.2642
297.1439
P =
0.0079 0 0
0 0.0079 0
0 0 0.0093
时间已过 0.012223 秒。