星图识别4-共线方程与非线性最小二乘法求滚转角

大概拖了一个月都没有进度，今天才找到正确的切入点。

即是这篇 CN100348460C - 一种基于星场的星敏感器校准方法 专利，这篇专利因最开始的一张非右手坐标系的图，被我打入冷宫，但经过自己的琢磨发现自己原先的想法应该是错的，于是顺着该专利仔细研究，尤其对下述式子尤其好奇，于是想在此介绍一下共线方程和高斯牛顿非线性最小二乘法。

共线方程参考 https://baike.baidu.com/item/%E5%85%B1%E7%BA%BF%E6%96%B9%E7%A8%8B/2055962 和 https://zhuanlan.zhihu.com/p/101549821 。

其实很好理解，其实我原本的计算过程也近似如此，只不过下式更为精简。

在专利中有此公式，053.pdf中也有此公式。我一直挂念于心，确实以自己的想法无法解出滚转角。

本以为像线性最小二乘法能解决，但仔细看该专利，结合查资料，用的是高斯牛顿非线性最小二乘法。

https://zhuanlan.zhihu.com/p/42383070

以星图1为例

%  main9.m
%  采用共线公式正向验证星图1从世界坐标到星敏坐标到像素坐标并画图
%  高斯牛顿非线性最小二乘算法求解滚转角

clc;clear;close all;
index = [1670
1477
1502
1631
1603
1453
1432
1492
1488
1648
1646
1566
1688
1655
1505
];
load("附件2  简易星表.mat");

f = 256/(tand(6));
A1 = xlsread('D:\文献\starimage\npmcm2019b\npmcm2019-B\附件3  8幅星图相关数据\xingtu01.xls') - 256;
xingtu1=[-A1(:,1), A1(:,2) ];

% newMatrix= [A1,repmat(f,size(A1,1),1)];
% A  =  [  cosd(star_data(index,3)).*cosd(star_data(index,2))   ,   cosd(star_data(index,3)).*sind(star_data(index,2))  ,  sind(star_data(index,3))  ];
% Y = [   (f  ./  (sqrt(f^2 + newMatrix(1:15,1).^2 + newMatrix(1:15,2).^2))  )];
% zhixiang = inv(A'*A)*A'*Y;
% E = (A*zhixiang - Y);
% RMS = sqrt(mse(E));

% if   zhixiang(2)/sqrt(1-zhixiang(3)^2) > 0 
%   a=acosd( zhixiang(1)  /  sqrt(1-((zhixiang(3))^2) )   );
% else 
%    a = 360-acosd( zhixiang(1)  /  sqrt(1-((zhixiang(3))^2) )   );
% end
% b=asind( zhixiang(3) );
corWorld =  [  cosd(star_data(index,3)).*cosd(star_data(index,2))   ,   cosd(star_data(index,3)).*sind(star_data(index,2))  ,  sind(star_data(index,3))  ];


syms a b fai;
R = [
-sind(a)*cosd(fai)-cosd(a)*sind(b)*sind(fai)      cosd(a)*cosd(fai)-sind(a)*sind(b)*sind(fai)      cosd(b)*sind(fai)
 sind(a)*sind(fai)-cosd(a)*sind(b)*cosd(fai)     -cosd(a)*sind(fai)-sind(a)*sind(b)*cosd(fai)      cosd(b)*cosd(fai)
 cosd(a)*cosd(b)                                  sind(a)*cosd(b)                                  sind(b)
];

x_ba = f .* ( R(1,1).*corWorld(:,1) +  R(1,2).*corWorld(:,2)  +  R(1,3).*corWorld(:,3)) ./ (R(3,1).*corWorld(:,1) +  R(3,2).*corWorld(:,2)  +  R(3,3).*corWorld(:,3));
y_ba = f .* ( R(2,1).*corWorld(:,1) +  R(2,2).*corWorld(:,2)  +  R(2,3).*corWorld(:,3)) ./ (R(3,1).*corWorld(:,1) +  R(3,2).*corWorld(:,2)  +  R(3,3).*corWorld(:,3));


A = [diff(x_ba,a) , diff(x_ba,b) , diff(x_ba,fai)];
B = [diff(y_ba,a) , diff(y_ba,b) , diff(y_ba,fai)];
% iter=[0;0;0];
iter=[109;34;0];

for i = 1 : 10
    
    p = [double(subs(x_ba , {a,b,fai},{iter(1),iter(2),iter(3)}))-xingtu1(:,1); double(subs(y_ba , {a,b,fai},{iter(1),iter(2),iter(3)})) - xingtu1(:,2)];
    M = [double(subs(A , {a,b,fai},{iter(1),iter(2),iter(3)})); double(subs(B , {a,b,fai},{iter(1),iter(2),iter(3)}))];
    wow = inv((M.') * M) * (M.')*p;
    iter = iter - wow
%     plot(i,iter(1));
    plot(i,iter(3),'-or'); %线性，颜色，标记
    hold on
end
disp('maxp=');
disp(max(p));
%  最后看各点的差值p即可