椭球拟合,最小二乘(加速度)

心不动则不痛 提交于 2020-02-15 19:40:19

最近在学习椭球拟合,最小二乘(加速度)的相关内容,把不错的几个学习参考链接放到下面:

三维空间中的椭球拟合与磁力计、加速度计校正

最小二乘估计及证明

平面二维任意椭圆数据拟合算法推导及程序实现详解

空间二次曲面数据拟合算法推导及仿真分析

IMU加速度、磁力计校正--椭球拟合

 

附上通过学习以上文章写出的测试代码;

%最小二乘的方法进行拟合
clear all;
close all
clc;
R = 2;         %球面半径

RX = 2;
RY = 10;
RZ = 20;

x0 = 100;      %球心x坐标
y0 = 1;        %球心y坐标
z0 = 76;       %球心z坐标

alfa = 0:pi/50:pi;
sita = 0:pi/50:2*pi;
num_alfa = length(alfa);
num_sita = length(sita);
x = zeros(num_alfa,num_sita);
y = zeros(num_alfa,num_sita);
z = zeros(num_alfa,num_sita);
for i = 1:num_alfa
    for j = 1:num_sita
        x(i,j) = x0+RX*sin(alfa(i))*cos(sita(j));
        y(i,j) = y0+RY*sin(alfa(i))*sin(sita(j));
        z(i,j) = z0+RZ*cos(alfa(i));
    end
end
x = reshape(x,num_alfa*num_sita,1);
y = reshape(y,num_alfa*num_sita,1);
z = reshape(z,num_alfa*num_sita,1);
figure;
plot3(x,y,z,'*');
title('生成的没有噪声的球面数据');
%加入均值为0的高斯分布噪声 
amp = 1;
x = x + amp*rand(num_alfa*num_sita,1);
y = y + amp*rand(num_alfa*num_sita,1);
z = z + amp*rand(num_alfa*num_sita,1);
figure;
plot3(x,y,z,'*');
title('加入噪声的球面数据');

x;
y;
z;
K = [y.^2 z.^2 x y z ones(length(x),1)];
Y = [-x.^2];
KT = K.';
X=inv(KT*K)*KT*Y;

RA = X(1,1);
RB = X(2,1);
RC = X(3,1);
RD = X(4,1);
RE = X(5,1);
RF = X(6,1);

x0
y0
z0
RX
RY
RZ

OX= -RC/2
OY=-RD/2/RA
OZ=-RE/2/RB
FRX=sqrt(OX^2+RA*OY^2+RB*OZ^2-RF)
FRY=sqrt(RX^2/RA)
FRZ=sqrt(RX^2/RB)

 

=========================>>

 

 

 

=========================>>

 

 

 

=========================>>

 

 

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!