摄影测量-空间前交、后交【精选文档】

摄影测量学第一章绪论1摄影测量是从非接触成像系统，通过记录、量测、分析与表达等处理，获取地球及其环境和其他物体的几何、属性等可靠信息的工艺、科学与技术。

2、摄影测量学的三个发展阶段：模拟摄影测量、解析摄影测量、数字摄影测量第二章单幅影像解析基础1像主点：摄影机主光轴（摄影方向）与像平面的交点，称为像片主点。

像主距：摄影机物镜后节点到像片主点的垂距称为摄影机主距，也叫像片主距（f）。

2、航空摄影：利用安装在航摄飞机上的航摄仪，在空中以预定的飞行高度度沿着事先制定好的航线飞行，按一定的时间间隔进行曝光摄影，获取整个测区的航摄像片。

空中摄影采用竖直摄影方式，即摄影瞬间摄影机物镜主光轴近似与地面垂直。

丄丄fm L H（m—像片比例尺分母，f—摄影机主距，H —平均高程面的摄影高度H=m • f）3、相对航高是指摄影机物镜相对于某一基准面的高度，称为摄影航高。

绝对航高是相对于平均海平面的航高，是指摄影机物镜在摄影瞬间的真实海拔高。

通过相对航高H与摄影地区地面平均高度H地计算得到：H绝=H+H地5、航向重叠：同一条航线内相邻像片之间的影像重叠称，重叠度一般要求在60%以上;旁向重叠：两相邻航带像片之间的影像重叠，重叠度要求在30%左右。

6、中心投影：当投影会聚于一点时，称为中心投影；正射投影：投影射线与投影平面成正交。

r中心投影：投影射线会聚于一点（投影射线的会聚点称投影中心）r斜投影：投影射线与投影平面成斜交投影i正射投影：投影射线与投影平面成正交7、 透视变换中的重要的点线面：① 由投影中心作像片平面的垂线，交像面于 0,称为像主点；像主点在地面上的对应点以O 表示，称为地主点。

② 由摄影中心作铅垂线交像片平面于点 n ,称为像底点；此铅垂线交地面于点 N ，称为地底点。

③ 过铅垂线SnN 和摄影方向SoO 的铅垂面称为主垂面（W ），主垂面即垂直于像平面 P ，又垂直于地平面 E ，也垂直于两平面的交线透视轴 TT 。

摄影测量学：利用光学摄影机摄取像片，通过像片来研究和确定被摄物体的形状、大小、位置和相互关系。

○1、摄影测量的按摄影机平台位置不同：航天摄影测量、航空摄影测量、地面摄影测量、水下摄影测量；○2、按摄影机平台与被摄目标距离的远近：航天摄影测量、航空摄影测量、地面摄影测量、近景摄影测量、显微摄影测量；○3、按用途：地形摄影、非地形摄影；摄影测量学的发展三个阶段：模拟摄影测量（1900~1960）、解析摄影测量（1950~1980）、数字摄影测量（1980~2000）。

框标装置：在固定不变的承片框上，四个边的中点各安置一个机械标志。

主光轴：组成物镜的各个透镜的光学中心位于同一直线上。

物方空间：以两平面来等价物镜组，则两平面将空间分为两个部分，物体所处空间即为物方空间。

像方空间：构像所处的空间。

摄影机主距：航空摄影机物镜中心至底片面的距离是固定值。

用f表示视场：光线通过物镜后，焦面上照度不均匀的光亮圆。

像场：影像相当清晰的一部分视场内的光亮圆。

视场角：由物镜后节点向视场边缘射出的光线所张开的角，用2a 表示像角：由镜头后节点向像场边缘射出的光线所张开的角。

摄影比例尺：航摄像片上一段为l 的影像与地面上相应线段的水平距离L之比，即1/m=l/L。

绝对航高：摄影瞬间摄影机物镜中心相对于平均海水面的航高。

相对航高：相对于其他某一基准面或某一点的高度。

摄影比例尺越大，像片地面的分辨率越高，有利于影像的解译与提高成图精度，但摄影比例尺过大，增加工作量及费用。

空中摄影过程，实质上是将地球表面上的地物，地貌等信息，穿过大气层，进入摄影机物镜，到达航摄胶片上形成影像的传输过程。

摄影基线：航线方向相邻两摄站点间的空间距离。

航向重叠：在同一条航线上，相邻两像片应有一定的范围的影像重叠。

旁向重叠：相邻航线也应有足够的重叠。

像片倾角，在摄影瞬间摄影机轴发生了倾角，摄影机轴与铅直方向的夹角a 一般要求倾角不大于2度，最大不超过3度。

摄影测量学实验报告实验一、单像空间后方交会学院：建测学院班级：测绘082姓名：肖澎学号： 15一．实验目的1.深入了解单像空间后方交会的计算过程；2.加强空间后方交会基本公式和误差方程式，法线方程式的记忆；3.通过上机调试程序加强动手能力的培养。

二．实验原理以单幅影像为基础，从该影像所覆盖地面范围内若干控制点和相应点的像坐标量测值出发，根据共线条件方程，求解该影像在航空摄影时刻的相片外方位元素。

三．实验内容1.程序图框图2.实验数据（1）已知航摄仪内方位元素f＝153.24mm，Xo＝Yo＝0。

限差0.1秒（2）已知4对点的影像坐标和地面坐标：3.实验程序using System;using System.Collections.Generic;using System.Linq;using System.Text;namespace ConsoleApplication3{class Program{static void Main(){//输入比例尺，主距，参与平参点的个数Console.WriteLine("请输入比例尺分母m：\r");string m1 = Console.ReadLine();double m = (double)Convert.ToSingle(m1);Console.WriteLine("请输入主距f：\r");string f1 = Console.ReadLine();double f = (double)Convert.ToSingle(f1);Console.WriteLine("请输入参与平差控制点的个数n：\r");string n1 = Console.ReadLine();int n = (int)Convert.ToSingle(n1);//像点坐标的输入代码double[] arr1 = new double[2 * n];//1.像点x坐标的输入for (int i = 0; i < n; i++){Console.WriteLine("请输入已进行系统误差改正的像点坐标的x{0}值：\r", i+1);string u = Console.ReadLine();for (int j = 0; j < n; j += 2){arr1[j] = (double)Convert.ToSingle(u);}}//2.像点y坐标的输入for (int i = 0; i < n; i++){Console.WriteLine("请输入已进行系统误差改正的像点坐标的y{0}值：\r", i+1);string v = Console.ReadLine();for (int j = 1; j < n; j += 2){arr1[j] = (double)Convert.ToSingle(v);}}//控制点的坐标输入代码double[,] arr2 = new double[n, 3];//1.控制点X坐标的输入for (int j = 0; j < n; j++){Console.WriteLine("请输入控制点在地面摄影测量坐标系的坐标的X{0}值：\r", j+1);string u = Console.ReadLine();arr2[j , 0] = (double)Convert.ToSingle(u);}//2.控制点Y坐标的输入for (int k = 0; k < n; k++){Console.WriteLine("请输入控制点在地面摄影测量坐标系的坐标的Y{0}值：\r", k+1);string v = Console.ReadLine();arr2[k , 1] = (double)Convert.ToSingle(v);}//3.控制点Z坐标的输入for (int p =0; p < n; p++){Console.WriteLine("请输入控制点在地面摄影测量坐标系的坐标的Z{0}值：\r", p+1);string w = Console.ReadLine();arr2[p , 2] = (double)Convert.ToSingle(w);}//确定外方位元素的初始值//1.确定Xs的初始值：double Xs0 = 0;double sumx = 0;for (int j = 0; j < n; j++){double h = arr2[j, 0];sumx += h;}Xs0 = sumx / n;//2.确定Ys的初始值：double Ys0 = 0;double sumy = 0;for (int j = 0; j < n; j++){double h = arr2[j, 1];sumy += h;}Ys0 = sumy / n;//3.确定Zs的初始值：double Zs0 = 0;double sumz = 0;for (int j = 0; j <= n - 1; j++){double h = arr2[j, 2];sumz += h;}Zs0 = sumz / n;doubleΦ0 = 0;doubleΨ0 = 0;double K0 = 0;Console.WriteLine("Xs0,Ys0,Zs0,Φ0,Ψ0,K0的值分别是：{0},{1},{2},{3},{4},{5}", Xs0, Ys0, Zs0, 0, 0, 0);//用三个角元素的初始值按（3-4-5）计算各方向余弦值，组成旋转矩阵,此时的旋转矩阵为单位矩阵I：double[,] arr3 = new double[3, 3];for (int i = 0; i < 3; i++)arr3[i, i] = 1;}double a1 = arr3[0, 0]; double a2 = arr3[0, 1]; double a3 = arr3[0, 2];double b1 = arr3[1, 0]; double b2 = arr3[1, 1]; double b3 = arr3[1, 2];double c1 = arr3[2, 0]; double c2 = arr3[2, 1]; double c3 = arr3[2, 2];/*利用线元素的初始值和控制点的地面坐标，代入共线方程（3-5-2），* 逐点计算像点坐标的近似值*///1.定义存放像点近似值的数组double[] arr4 = new double[2 * n];//----------近似值矩阵//2.逐点像点坐标计算近似值//a.计算像点的x坐标近似值（x）for (int i = 0; i < 2 * n; i += 2){for (int j = 0; j < n; j++){arr4[i] = -f * (a1 * (arr2[j, 0] - Xs0) + b1 * (arr2[j, 1] - Ys0) + c1 * (arr2[j, 2] - Zs0)) / (a3 * (arr2[j, 0] - Xs0) + b3 * (arr2[j, 1] - Ys0) + c3 * (arr2[j, 2] - Zs0)); }}//b.计算像点的y坐标近似值（y）for (int i = 1; i < 2 * n; i += 2){for (int j = 0; j < n; j++){arr4[i] = -f * (a2 * (arr2[j, 0] - Xs0) + b2 * (arr2[j, 1] - Ys0) + c2 * (arr2[j, 2] - Zs0)) / (a3 * (arr2[j, 0] - Xs0) + b3 * (arr2[j, 1] - Ys0) + c3 * (arr2[j, 2] - Zs0)); }}//逐点计算误差方程式的系数和常数项，组成误差方程：double[,] arr5 = new double[2 * n, 6]; //------------系数矩阵（A）//1.计算dXs的系数for (int i = 0; i < 2 * n; i += 2){arr5[i, 0] = -1 / m; //-f/H == -1/m}//2.计算dYs的系数for (int i = 1; i < 2 * n; i += 2){arr5[i, 1] = -1 / m; //-f/H == -1/m}//3.a.计算误差方程式Vx中dZs的系数for (int i = 0; i < 2 * n; i += 2)arr5[i, 2] = -arr1[i] / m * f;}//3.b.计算误差方程式Vy中dZs的系数for (int i = 1; i < 2 * n; i += 2){arr5[i, 2] = -arr1[i] / m * f;}//4.a.计算误差方程式Vx中dΦ的系数for (int i = 0; i < 2 * n; i += 2){arr5[i, 3] = -f * (1 + arr1[i] * arr1[i] / f * f);}//4.a.计算误差方程式Vy中dΦ的系数for (int i = 1; i < 2 * n; i += 2){arr5[i, 3] = -arr1[i - 1] * arr1[i] / f;}//5.a.计算误差方程式Vx中dΨ的系数for (int i = 0; i < 2 * n; i += 2){arr5[i, 4] = -arr1[i] * arr1[i + 1] / f;}//5.b.计算误差方程式Vy中dΨ的系数for (int i = 1; i < 2 * n; i += 2){arr5[i, 4] = -f * (1 + arr1[i] * arr1[i] / f * f);}//6.a.计算误差方程式Vx中dk的系数for (int i = 0; i < 2 * n; i += 2){arr5[i, 5] = arr1[i + 1];}//6.b.计算误差方程式Vy中dk的系数for (int i = 1; i < 2 * n; i += 2){arr5[i, 5] = -arr1[i - 1];}//定义外方位元素组成的数组double[] arr6 = new double[6];//--------------------外方位元素改正数矩阵（X）//定义常数项元素组成的数组double[] arr7 = new double[2 * n];//-----------------常数矩阵（L）//计算lx的值for (int i = 0; i < 2 * n; i += 2)arr7[i] = arr1[i] - arr4[i]; //将近似值矩阵的元素代入}//计算ly的值for (int i = 1; i <= 2 * (n - 1); i += 2){arr7[i] = arr1[i] - arr4[i]; //将近似值矩阵的元素代入}/* 对于所有像点的坐标观测值，一般认为是等精度量测，所以权阵P为单位阵.所以X=(ATA)-1ATL *///1.计算ATdouble[,] arr5T = new double[6, 2 * n];for (int i = 0; i < 6; i++){for (int j = 0; j < 2 * n; j++){arr5T[i, j] = arr5[j, i];}}//A的转置与A的乘积,存放在arr5AA中double[,] arr5AA = new double[6, 6];for (int i = 0; i < 6; i++){for (int j = 0; j < 6; j++){arr5AA[i, j] = 0;for (int l = 0; l < 2 * n; l++){arr5AA[i, j] += arr5T[i, l] * arr5[l, j];}}}nijuzhen(arr5AA);//arr5AA经过求逆后变成原矩阵的逆矩阵//arr5AA * arr5T存在arr5AARATdouble[,] arr5AARAT = new double[6, 2 * n];for (int i = 0; i < 6; i++){for (int j = 0; j < 2 * n; j++){arr5AARAT[i, j] = 0;for (int p = 0; p < 6; p++){arr5AARAT[i, j] += arr5AA[i, p] * arr5T[p, j];}}}//计算arr5AARAT x L，存在arrX中double[] arrX = new double[6];for (int i = 0; i < 6; i++){for (int j = 0; j < 1; j++){arrX[i] = 0;for (int vv = 0; vv < 6; vv++){arrX[i] += arr5AARAT[i, vv] * arr7[vv];}}}//计算外方位元素值double Xs, Ys, Zs, Φ, Ψ, K;Xs = Xs0 + arrX[0];Ys = Ys0 + arrX[1];Zs = Zs0 + arrX[2];Φ = Φ0 + arrX[3];Ψ = Ψ0 + arrX[4];K = K0 + arrX[5];for (int i = 0; i <= 2; i++){Xs += arrX[0];Ys += arrX[1];Zs += arrX[2];Φ += arrX[3];Ψ += arrX[4];K += arrX[5];}Console.WriteLine("Xs,Ys,Zs,Φ,Ψ,K的值分别是：{0},{1},{2},{3},{4},{5}", Xs0, Ys0, Zs0, Φ, Ψ, K);Console.Read();}//求arr5AA的逆矩public static double[,] nijuzhen(double[,] a) {double[,] B = new double[6, 6];int i, j, k;int row = 0;int col = 0;double max, temp;int[] p = new int[6];for (i = 0; i < 6; i++){p[i] = i;B[i, i] = 1;}for (k = 0; k < 6; k++){//找主元max = 0; row = col = i;for (i = k; i < 6; i++){for (j = k; j < 6; j++){temp = Math.Abs(a[i, j]);if (max < temp){max = temp;row = i;col = j;}}}//交换行列，将主元调整到k行k列上if (row != k){for (j = 0; j < 6; j++){temp = a[row, j];a[row, j] = a[k, j];a[k, j] = temp;temp = B[row, j];B[row, j] = B[k, j];B[k, j] = temp;i = p[row]; p[row] = p[k]; p[k] = i; }if (col != k){for (i = 0; i < 6; i++){temp = a[i, col];a[i, col] = a[i, k];a[i, k] = temp;}}//处理for (j = k + 1; j < 6; j++){a[k, j] /= a[k, k];}for (j = 0; j < 6; j++){B[k, j] /= a[k, k];a[k, k] = 1;}for (j = k + 1; j < 6; j++){for (i = 0; j < k; i++){a[i, j] -= a[i, k] * a[k, j];}for (i = k + 1; i < 6; i++){a[i, j] -= a[i, k] * a[k, j];}}for (j = 0; j < 6; j++){for (i = 0; i < k; i++){B[i, j] -= a[i, k] * B[k, j];}for (i = k + 1; i < 6; i++){B[i, j] -= a[i, k] * B[k, j];}for (i = 0; i < 6; i++) {a[i, k] = 0;a[k, k] = 1;}}//恢复行列次序for (j = 0; j < 6; j++){for (i = 0; i < 6; i++) {a[p[i], j] = B[i, j]; }}for (i = 0; i < 6; i++){for (j = 0; j < 6; j++) {a[i, j] = a[i, j];}}return a;}4．实验结果四．实验总结此次实验让我深入了解单像空间后方交会的计算过程，加强了对空间后方交会基本公式和误差方程式，法线方程式的记忆。

一．名词解释1.摄影测量的定义.2.中心投影：投影光线会聚于一点的投影称为中心投影。

3.摄影基线：航线方向相邻两个摄影站点间的空间距离4.相对定向：相对定向：根据立体像对内在的几何关系恢复两张像片之间的相对位置和姿态，使同名光线对对相交，建立与地面相似的立体模型。

即确定一个立体像对两像片的相对位置。

5.像片旋偏角：摄影瞬间摄影机的主光轴近似与 地面垂直，偏离铅垂线的夹角小于2度～3度，夹角为像片旋偏角6.单像空间后方交会：利用至少三个已知地面控制点的坐标，与其影像上对应三个像点的影像坐标，根据共线条件方程，反求该像片的外方位元素。

7.空间前方交会：由立体像对中两张像片的内、外方位元素和像点坐标来确定相应地面点的地面坐标的方法，称为空间前方交会。

8.数字影像内定向：同一像点的像平面坐标与其扫描坐标不相等，需要加以换算，这种换算称为数字影像内定向。

9.摄影机主光轴：物镜后节点作框标平面的垂线10.空间后方交会：航摄像片可以在摄影之后，利用一定数量的地面控制点，根据共线条件方程或反求像片的外方位元素这种方法称为单张像片的空间后方交会。

11.立体像对：相邻摄站获取的具有一定重叠度的两张影像。

12.解析法绝对定向：借助地面控制点，将相对定向模型进行缩放、平移和旋转，使其达到绝对位置。

二．填空1.摄影测量学的发展经过了模拟摄影测量、解析摄影测量、数字摄影测量三个阶段。

2.摄影测量常用的坐标系统有：像平面坐标系、像空间坐标系、像空间辅助坐标系 、摄影测量坐标系、地面测量坐标系、.3.共线方程表达的是像点、投影中心与地面点之间关系。

4.一张像片的内方位元素包括：x0、 y0 、 f ；外方位元素包括：三个线元素(Xs 、Ys 、Zs ）：描述摄影中心的空间坐标值；三个角元素(ϕ、ω、κ) ) ：描述像片的空间姿态。

5.解析绝对定向需要量测 2 个平高和 1 个高程以上的控制点，一般是在模型四个角布设四个控制点。

空间后交-前交程序设计(实验报告)姓名：班级：学号：时间：空间后交-前交程序设计一、实验目的用 C 、VB或MATLAB语言编写空间后方交会-空间前方交会程序⑴提交实习报告：程序框图、程序源代码、计算结果、体会⑵计算结果：像点坐标、地面坐标、单位权中误差、外方位元素及其精度二、实验数据f=150.000mm，x0=0，y0=0三、实验思路1.利用空间后方交会求左右像片的外方位元素(1).获取m(于像片中选取两点，于地面摄影测量坐标系中选取同点，分别计算距离，距离比值即为m)，x,y,f,X,Y,Z(2).确定未知数初始值Xs,Ys,Zs,q,w,k(3).计算旋转矩阵R(4).逐点计算像点坐标的近似值（x），（y）(5).组成误差方程式(6).组成法方程式(7).解求外方位元素(8).检查是否收敛，即将求得的外方位元素的改正数与规定限差比较，小于限差即终止；否则用新的近似值重复步骤（3）-（7）2.利用求出的外方位元素进行空间前交，求出待定点地面坐标(1).用各自像片的角元素计算出左、右像片的方向余弦值，组成旋转矩阵R1,R2(2).根据左、右像片的外方位元素，计算摄影基线分量Bx，By，Bz(3).计算像点的像空间辅助坐标（X1,Y1,Z1）和（X2,Y2,Z2）(4).计算点投影系数N1和N2(5).计算未知点的地面摄影测量坐标四、实验过程⑴程序框图函数AandL%求间接平差时需要的系数%%%已知%a=像点坐标x,b=像点坐标y，f内方位元素主距%φ=q,ψ=w,κ=k%像空间坐标系X,Y,Z%地面摄影测量坐标系Xs,Ys,Zsfunction [A1,L1,A2,L2]=AandL(a,b,f,q,w,k,X,Y,Z,Xs,Ys,Zs) %%%%%%%%%%%选择矩阵元素a1=cos(q)*cos(k)-sin(q)*sin(w)*sin(k);a2=-cos(q)*sin(k)-sin(q)*sin(w)*cos(k);a3=-sin(q)*cos(w);b1=cos(w)*sin(k);b2=cos(w)*cos(k);b3=-sin(w);c1=sin(q)*cos(k)+cos(q)*sin(w)*sin(k);c2=-sin(q)*sin(k)+cos(q)*sin(w)*cos(k);c3=cos(q)*cos(w);%%%%%%%共线方程的分子分母X_=a1*(X-Xs)+b1*(Y-Ys)+c1*(Z-Zs);Y_=a2*(X-Xs)+b2*(Y-Ys)+c2*(Z-Zs);Z_=a3*(X-Xs)+b3*(Y-Ys)+c3*(Z-Zs);%%%%%%%近似值x=-f*X_/Z_;y=-f*Y_/Z_;%%%%%%%A组成L组成a11=1/Z_*(a1*f+a3*x);a12=1/Z_*(b1*f+b3*x);a13=1/Z_*(c1*f+c3*x);a21=1/Z_*(a2*f+a3*y);a22=1/Z_*(b2*f+b3*y);a23=1/Z_*(c2*f+c3*y);a14=y*sin(w)-(x/f*(x*cos(k)-y*sin(k))+f*cos(k))*cos(w);a15=-f*sin(k)-x/f*(x*sin(k)+y*cos(k)); a16=y;a24=-x*sin(w)-(y/f*(x*cos(k)-y*sin(k))-f*sin(k))*cos(w);a25=-f*cos(k)-y/f*(x*sin(k)+y*cos(k));a26=-x;lx=a-x;ly=b-y;%%%%%%%%%组成一个矩阵，并返回A1=[a11,a12,a13,a14,a15,a16];A2=[a21,a22,a23,a24,a25,a26];L1=lx;L2=ly;函数deg2dms%%%%%%%%角度转度分秒function y=deg2dms(x)a=floor(x);b=floor((x-a)*60);c=(x-a-b/60)*3600;y=a+(b/100)+(c/10000);函数dms2deg%%%%%度分秒转度function y=dms2deg(x)a=floor(x);b=floor((x-a)*100);c=(x-a-b/100)*10000;y=a+b/60+c/3600;函数ok%%%%%%%%%%%%%%目的是为了保证各取的值的有效值%%xy为n*1,a为1*nfunction result=ok(xy,a)format short gi=size(xy,1);for n=1:io=xy(n)-floor(xy(n,1));o=round(o*(10^a(n)))/(10^a(n));xy(n,1)=floor(xy(n,1))+o;endformat long gresult=xy;函数rad2dmsxy%%%%求度分秒表现形式的三个外方位元素，三个角度function xydms=rad2dmsxy(xy)[a,b,c,d,e,f]=testvar(xy);d=deg2dms(rad2deg(d));e=deg2dms(rad2deg(e));f=deg2dms(rad2deg(f));xydms=[a,b,c,d,e,f]';函数spacehoujiao%%%%%%%空间后交%%% f%%输入p（2*n,1）%%像点坐标x,y,X,Y,Z,均为（n,1）function [xy,m,R]=spacehoujiao(p,x,y,f,X,Y,Z)format long;%%%%%权的矢量化,这是等精度时的，如果非，将函数参数改为P P=diag(p);%%求nj=size(X,2);%%初始化Xs=0;Ys=0;Zs=0;for n=1:jXs=Xs+X(n);Ys=Ys+Y(n);Zs=Zs+Z(n);endSx=sqrt((x(2)-x(1))^2+(y(2)-y(1))^2);%%%%两像点之间距离Sd=sqrt((X(2)-X(1))^2+(Y(2)-Y(1))^2);%%%%两地面控制点之间距离m=Sd/Sx; %%%%图像比例系数Xs=Xs/j;Ys=Ys/j;Zs=m*f+Zs/j;m0=0;q=0;w=0;k=0;i=0;a=rand(2*j,6);l=rand(2*j,1);%%%%for n=1:j[a(2*n-1,:),l(2*n-1,1),a(2*n,:),l(2*n,1)]=AandL(x(n),y(n),f,q,w,k,X(n),Y(n),Z(n ),Xs,Ys,Zs);enddet=inv(a'*P*a)*transpose(a)*P*l;%%%%%%%%%循环体while 1%%%%%%%%%%%%%%%%[dXs,dYs,dZs,dq,dw,dk]=testvar(det);detXs=abs(dXs);detYs=abs(dYs);detZs=abs(dZs);detq=abs(dq);detw=abs(dw);detk=abs(dk);%%%%%%%%%if((detXs<0.01)&&(detYs<0.01)&&(detZs<0.01)&&(detq<pi/648000)&&(detw<pi/648000)&& (detk<pi/648000))break;elseV=(a*det-l);Q=inv(a'*P*a);m0=m0+sqrt((V'*P*V)/(2*j-6));%%m0需要每次的改正数算出来相加%%%Xs=Xs+dXs;Ys=Ys+dYs;Zs=Zs+dZs;q=q+dq;w=w+dw;k=k+dk;%%%for n=1:j[a(2*n-1,:),l(2*n-1,1),a(2*n,:),l(2*n,1)]=AandL(x(n),y(n),f,q,w,k,X(n),Y(n),Z(n ),Xs,Ys,Zs);enddet=inv(a'*P*a)*transpose(a)*P*l;i=i+1;%%%%end%%%end[dXs,dYs,dZs,dq,dw,dk]=testvar(det);detXs=abs(dXs);detYs=abs(dYs);detZs=abs(dZs);detq=abs(dq);detw=abs(dw);detk=abs(dk);V=(a*det-l);Q=inv(a'*P*a);m0=m0+sqrt((V'*P*V)/(2*n-6));%%%Xs=Xs+dXs;Ys=Ys+dYs;Zs=Zs+dZs;q=q+dq;w=w+dw;k=k+dk;%%%%%%%%%%%%%可以输出迭代次数的i%%%%%%%%%%%%Xs,Ys,Zs,q,w,k,i,dXs,dYs,dZs,dq,dw,dk,detXs,detYs,detZs %%%%%%%%%%%精度mo=m0*sqrt(Q);m=[mo(1,1),mo(2,2),mo(3,3),mo(4,4),mo(5,5),mo(6,6)]';[mXs,mYs,mZs,mq,mw,mk]=testvar(m);%%%%%%%%%输出xy=[Xs,Ys,Zs,q,w,k]';%%输出（6,1）的外方位元素m=[m0,mXs,mYs,mZs,mq,mw,mk]';%%单位误差，各元素中误差R=xyR(xy);%%旋转矩阵函数spaceqianjiao%%空间前交%输入f%输入x1,y1,x2,y2,R1,R2,xy1,xy2 （n,1）%输出X,Y,Z （n,1）function [X,Y,Z]=spaceqianjiao(x1,y1,x2,y2,f,R1,R2,xy1,xy2) i=size(x1,2);[Xs1,Ys1,Zs1,q1,w1,k1]=testvar(xy1);[Xs2,Ys2,Zs2,q2,w2,k2]=testvar(xy2);for n=1:i[X1(n),Y1(n),Z1(n)]=testvar(R1*[x1(n),y1(n),-f]');[X2(n),Y2(n),Z2(n)]=testvar(R2*[x2(n),y2(n),-f]');Bx=Xs2-Xs1;By=Ys2-Ys1;Bz=Zs2-Zs1;N1=(Bx*Z2(n)-Bz*X2(n))/(X1(n)*Z2(n)-X2(n)*Z1(n));N2=(Bx*Z1(n)-Bz*X1(n))/(X1(n)*Z2(n)-X2(n)*Z1(n));X(n)=Xs1+N1*X1(n);Z(n)=Zs1+N1*Z1(n);Y(n)=0.5*((Ys1+N1*Y1(n))+(Ys2+N2*Y2(n)));end函数testvar%分割矩阵。