加速度计的简单无依托标定

在进行非线性最小二乘时，由于没有测量真值，需要自己找"真值"以完成误差的计算，所以利用测量值的约束，将g^2作为“真值”，对误差进行变形，从而求得雅各比矩阵从而完成迭代计算。

代码如下：
noise.m为手动生成真值与观测值
LM.m为非线性最小二乘的方法
opt.m为最小二乘的方法

noise.m
clear all;
clc;
%%
%定义初始零偏
bx=0.0124;
by=0.0424;
bz=0.0324;
b=[bx by bz];
%%
%定义转移矩阵
Kx=1.01;
Ky=0.99;
Kz=1.02;
K=[Kx 0 0;0 Ky 0;0 0 Kz];
%%
%生成随机真值
A=rand(100,3);
S=sum(A.^2,2);
g=9.8;
S1=sqrt(g^2./S);
for i=1:100
    A(i,:)=A(i,:)*S1(i);
end
%%
%生成真值
B=zeros(100,3);
A_a=zeros(100,3);
a=zeros(100,3);
for i=1:100
    B(i,:)=b;
    A_a(i,:)=A(i,:)-B(i,:);
end
Kinv=inv(K)
for i=1:100    
 a(i,:)=(Kinv*A_a(i,:)')';%理想观测值
end
Noise=wgn(100,3,0)/10000;
a_n=a+Noise;%真实观测值
%%保存结果
save a a;
save a_n a_n;
save K K;
save Kinv Kinv;
save b b;

LM.m
%%非线最小二乘
clear all;
clc;
load('a_n.mat');
load('a.mat');
load('b.mat');
g=9.8;
%%定义方程
%定义残差
%g^2-sqrt(Ax^2+Ay^2+Az^2)
Kz=2;
Ky=2;
Kx=2;
bx=1;
by=1;
bz=1;
K=[Kx Ky Kz];
J=zeros(100,6);
X=[Kx Ky Kz bx by bz];
DZ=zeros(100,1);
for i=1:100
    
    J(i,1)=2*X(1)*a(i,1)^2+2*a(i,1)*X(4);
    J(i,2)=2*X(2)*a(i,2)^2+2*a(i,2)*X(5);
    J(i,3)=2*X(3)*a(i,3)^2+2*a(i,3)*X(6);
    J(i,4)=2*X(1)*a(i,1)+2*X(4);
    J(i,5)=2*X(2)*a(i,2)+2*X(5);
    J(i,6)=2*X(2)*a(i,3)+2*X(6);
    DZ(i,1)=9.8^2-X(1)^2*a(i,1)^2-2*X(1)*a(i,1)*X(4)-X(2)^2-X(2)^2*a(i,2)^2-2*X(2)*a(i,2)*X(5)-X(3)^2*a(i,3)^2-2*X(3)*a(i,3)*X(6)-X(4)^2-X(5)^2-X(6)^2;
end 
N=200;%迭代次数
S=0.000000000000000000001;
%%误差允许范围，可选
for k=1:N;
    DX=inv(J'*J)*J'*DZ;
    if sum(DX.^2)/(sum(X.^2))<S
        break
    end
    X=X+DX';
    for j=1:100
       DZ(j,1)= 9.8^2-X(1)^2*a(j,1)^2-2*X(1)*a(j,1)*X(4)-X(2)^2-X(2)^2*a(j,2)^2-2*X(2)*a(j,2)*X(5)-X(3)^2*a(j,3)^2-2*X(3)*a(j,3)*X(6)-X(4)^2-X(5)^2-X(6)^2;
    end
    
end
result=X;

opt.m
clear all;
clc;
load('a_n.mat');
load('a.mat');
load('b.mat');
g=9.8;
A=zeros(100,6);
for i=1:100
ax=a_n(i,1);
ay=a_n(i,2);
az=a_n(i,3);
    A(i,1)=ax^2;
    A(i,2)=ax;
    A(i,3)=ay^2;
    A(i,4)=ay;
    A(i,5)=az^2;
    A(i,6)=az;
end

Z=ones(100,1);
p=-inv(A'*A)*A'*Z;
p1=p(1,1);
p2=p(2,1);
p3=p(3,1);
p4=p(4,1);
p5=p(5,1);
p6=p(6,1);
PP=[1-4*p1/(p2^2) 1 1;1 1-4*p3/(p4^2) 1;1 1 1-4*p5/(p6^2)];
G=[g^2;g^2;g^2];
B=inv(PP)*G;
bx2=B(1);
by2=B(2);
bz2=B(3);
Kx=sqrt(p1*(sum(B)-g^2));
Ky=sqrt(p3*(sum(B)-g^2));
Kz=sqrt(p5*(sum(B)-g^2));
bx=p2*(sum(B)-g^2)/(2*Kx);
by=p4*(sum(B)-g^2)/(2*Ky);
bz=p6*(sum(B)-g^2)/(2*Kz);
result=[Kx Ky Kz bx by bz];