加速度计无依托标定（线性最小二乘、非线性最小二乘、递归最小二乘）

最新推荐文章于 2023-02-22 18:28:11 发布

Leon_Jay

最新推荐文章于 2023-02-22 18:28:11 发布

阅读量1.6k

点赞数 1

本文链接：https://blog.csdn.net/qq_41399267/article/details/88827883

版权

%%线性最小二乘

clear all;
clc;
%%
ax=zeros(500,1);
ay=zeros(500,1);
az=zeros(500,1);
b=zeros(3,1);
k=zeros(3,1);
a_x=zeros(500,1);
a_y=zeros(500,1);
a_z=zeros(500,1);
%%生成初始零偏
for i=1:3;
b(i)=rand;
end
%%

%生成初始标定因数
for i=1:3;
k(i)=1+rand;
end
%%
%生成随机真值
n1 = wgn(500,1,0);
n2 = wgn(500,1,0);
n3 = wgn(500,1,0);
for i = 1:500
ax(i)=sqrt(n1(i)^2*9.8*9.8/(n1(i)^2+n2(i)^2+n3(i)^2));
ay(i)=sqrt(n2(i)^2*9.8*9.8/(n1(i)^2+n2(i)^2+n3(i)^2));
az(i)=sqrt(n3(i)^2*9.8*9.8/(n1(i)^2+n2(i)^2+n3(i)^2));
end
% for i=1:5
% ax(i)=9.5;
% ay(i)=rand(1,1)*2.4026;
% az(i)=sqrt(9.8*9.8-9.5*9.5-ay(i)*ay(i));
% ax(i+5)=0.5;
% ay(i+5)=rand(1,1)*9.787;
% az(i+5)=sqrt(9.8*9.8-0.5*0.5-ay(i+5)*ay(i+5));
% ax(i+10)=2.5;
% az(i+10)=rand(1,1)*9.1537;
% ay(i+10)=sqrt(9.8*9.8-3.5*3.5-az(i+10)*az(i+10));
% ax(i+15)=4.5;
% ay(i+15)=rand(1,1)*8.705;
% az(i+15)=sqrt(9.8*9.8-4.5*4.5-ay(i+15)*ay(i+15));
% end
%%
%映射到测量值
noise1 = wgn(500,1,0);
noise2 = wgn(500,1,0);
noise3 = wgn(500,1,0);
for i=1:50;
a_x(i)=(ax(i)-b(1))/k(1)+noise1(1)/10;
a_y(i)=(ay(i)-b(2))/k(2)+noise1(2)/10;
a_z(i)=(az(i)-b(3))/k(3)+noise1(3)/10;
end
%%
%构造A矩阵
A=ones(500,6);
for i =1:500;
A(i,1)=a_x(i)^2;
A(i,2)=a_x(i);
A(i,3)=a_y(i)^2;
A(i,4)=a_y(i);
A(i,5)=a_z(i)^2;
A(i,6)=a_z(i);
end
%%
%构造P矩阵
Z=ones(500,1);
p = -(inv(A'*A)*A')*Z;
%%
%构造估计的零偏平方和
b_2=ones(3,1);
tran=ones(3,3)
tran(1,1)=1-4*p(1)/(p(2)*p(2));
tran(2,2)=1-4*p(3)/(p(4)*p(4));
tran(3,3)=1-4*p(5)/(p(6)*p(6));

% trans=[1-4*p(1)/(p(2)*p(2)) 1 1 ;
% 1 1-4*p(3)/(p(4)*p(4)) 1;
% 1 1 1-4*p(5)/(p(6)*p(6))];
g2=[9.8^2;
9.8^2;
9.8^2]
b_2=(inv(tran))*g2;
%% 求解刻度因数
k_x=sqrt(p(1)*(b_2(1)^2+b_2(2)^2+b_2(3)^2-9.8*9.8));
k_y=sqrt(p(3)*(b_2(1)^2+b_2(2)^2+b_2(3)^2-9.8*9.8));
k_z=sqrt(p(5)*(b_2(1)^2+b_2(2)^2+b_2(3)^2-9.8*9.8));

%%
%%求解偏移
b_1=ones(3,1);
b_1(1)=p(2)*(b_2(1)+b_2(2)+b_2(3)-9.8*9.8)/2/k_x;
b_1(2)=p(4)*(b_2(1)+b_2(2)+b_2(3)-9.8*9.8)/2/k_y;
b_1(3)=p(6)*(b_2(1)+b_2(2)+b_2(3)-9.8*9.8)/2/k_z;
%%
err =0;
for i = 1:3
err =(b(i)-b_1(i))/b(i)+err;

end

%%非线性

clear all;
clc;
%%
ax=zeros(500,1);
ay=zeros(500,1);
az=zeros(500,1);
b=zeros(3,1);
k=zeros(3,1);
X=zeros(6,1);
a_x=zeros(500,1);
a_y=zeros(500,1);
a_z=zeros(500,1);
%%生成初始零偏
for i=1:3;
b(i)=rand;
end
%%

%生成初始标定因数
for i=1:3;
k(i)=1+rand;
end
%%
%生成随机真值
n1 = wgn(500,1,0);
n2 = wgn(500,1,0);
n3 = wgn(500,1,0);
for i = 1:500;
ax(i)=sqrt(n1(i)^2*9.8*9.8/(n1(i)^2+n2(i)^2+n3(i)^2));
ay(i)=sqrt(n2(i)^2*9.8*9.8/(n1(i)^2+n2(i)^2+n3(i)^2));
az(i)=sqrt(n3(i)^2*9.8*9.8/(n1(i)^2+n2(i)^2+n3(i)^2));
end

X_inc=iter(X, a_x, a_y, a_z);
X=X+X_inc;

while (normest(X_inc)/normest(X)>0.00001)
X_inc=iter(X, a_x, a_y, a_z);
X=X+X_inc;
end

%%非线性迭代函数iter()

function [X_inc]= iter(X, a_x, a_y, a_z)
F_X=ones(500,1);
for i=1:500;
F_X(i) = (X(1)*a_x(i)+X(4))^2+(X(2)*a_y(i)+X(5))^2+(X(3)*a_z(i)+X(6))^2;
end
%%构造雅各比矩阵
jj = zeros(500,6);
for i=1:500;
jj(i,1) = 2*X(1)*a_x(i)^2+2*a_x(i)*X(4);
jj(i,2) = 2*X(2)*a_y(i)^2+2*a_y(i)*X(5);
jj(i,3) = 2*X(3)*a_z(i)^2+2*a_z(i)*X(6);
jj(i,4) = 2*X(4)+2*X(1)*a_x(i);
jj(i,5) = 2*X(5)+2*X(2)*a_y(i);
jj(i,6) = 2*X(6)+2*X(3)*a_z(i);

end
%%构造增量
g2 = ones(500,1);
for i = 1:500;
g2(i) = 9.8^2;
end

Z_inc= zeros(500,1);
for i = 1:500;
Z_inc(i) = g2(i)-F_X(i);
end
% X_inc = zeros(6,1);
X_inc = (inv(jj'*jj))*jj'*Z_inc;
end

%%递推最小二乘

clear all;
clc;
%%
ax=zeros(500,1);
ay=zeros(500,1);
az=zeros(500,1);
b=zeros(3,1);
k=zeros(3,1);
% X=zeros(6,1);
a_x=zeros(500,1);
a_y=zeros(500,1);
a_z=zeros(500,1);
%%生成初始零偏
for i=1:3;
b(i)=rand;
end
%%

%映射到测量值
noise1 = wgn(500,1,0);
noise2 = wgn(500,1,0);
noise3 = wgn(500,1,0);
for i=1:500;
a_x(i)=(ax(i)-b(1))/k(1)+noise1(1)/10;
a_y(i)=(ay(i)-b(2))/k(2)+noise1(2)/10;
a_z(i)=(az(i)-b(3))/k(3)+noise1(3)/10;
end
%%
%构造估计矩阵F(X),初始化迭代参数
% F_X=ones(20,1);
% for i =1:3;
% X(i)= k(i);
% X(i+3) = b(i);
% end
h=[a_x(1)*a_x(1),a_x(1),a_y(1)*a_y(1),a_y(1),a_z(1)*a_z(1),a_z(1);
a_x(2)*a_x(2),a_x(2),a_y(2)*a_y(2),a_y(2),a_z(2)*a_z(2),a_z(2);
a_x(3)*a_x(3),a_x(3),a_y(3)*a_y(3),a_y(3),a_z(3)*a_z(3),a_z(3);
a_x(4)*a_x(4),a_x(4),a_y(4)*a_y(4),a_y(4),a_z(4)*a_z(4),a_z(4);
a_x(5)*a_x(5),a_x(5),a_y(5)*a_y(5),a_y(5),a_z(5)*a_z(5),a_z(5);
a_x(6)*a_x(6),a_x(6),a_y(6)*a_y(6),a_y(6),a_z(6)*a_z(6),a_z(6)];
x = -1*inv(h'*h)*h'*ones(6,1);
for i=7:500
for j = 1:i
H(j,1)=a_x(j)*a_x(j);
H(j,2)=a_x(j);
H(j,3)=a_y(j)*a_y(j);
H(j,4)=a_y(j);
H(j,5)=a_z(j)*a_z(j);
H(j,6)=a_z(j);
end
p=inv(H(1:i-1,:)'*H(1:i-1,:));
x = x+p*H(i,:)'*(inv(H(i,:)*p*H(i,:)'+1))*(-1-H(i,:)*x);
end

%构造估计的零偏平方和
b_2=ones(3,1);
tran=ones(3,3)
tran(1,1)=1-4*x(1)/(x(2)*x(2));
tran(2,2)=1-4*x(3)/(x(4)*x(4));
tran(3,3)=1-4*x(5)/(x(6)*x(6));

% trans=[1-4*p(1)/(p(2)*p(2)) 1 1 ;
% 1 1-4*p(3)/(p(4)*p(4)) 1;
% 1 1 1-4*p(5)/(p(6)*p(6))];
g2=[9.8^2;
9.8^2;
9.8^2]
b_2=(inv(tran))*g2;
%% 求解刻度因数
k_x=sqrt(x(1)*(b_2(1)^2+b_2(2)^2+b_2(3)^2-9.8*9.8));
k_y=sqrt(x(3)*(b_2(1)^2+b_2(2)^2+b_2(3)^2-9.8*9.8));
k_z=sqrt(x(5)*(b_2(1)^2+b_2(2)^2+b_2(3)^2-9.8*9.8));

%%
%%求解偏移
b_1=ones(3,1);
b_1(1)=x(2)*(b_2(1)+b_2(2)+b_2(3)-9.8*9.8)/2/k_x;
b_1(2)=x(4)*(b_2(1)+b_2(2)+b_2(3)-9.8*9.8)/2/k_y;
b_1(3)=x(6)*(b_2(1)+b_2(2)+b_2(3)-9.8*9.8)/2/k_z;
for i = 1:3
err =(b(i)-b_1(i))/b(i);

end

% X_inc=iter(X, a_x, a_y, a_z);
% X=X+X_inc;
%
% while (normest(X_inc)/normest(X)>0.00001)
% X_inc=iter(X, a_x, a_y, a_z);
% X=X+X_inc;
% end