本文共 12259 字,大约阅读时间需要 40 分钟。
关注即可了解更多相关知识。
欢迎转发、收藏、友善交流。
Givens rotation 逆时针
[ c − s s c ] \begin{bmatrix}c &-s\\s &c\end{bmatrix} [cs−sc] Jacobi rotation 顺时针 [ c s − s c ] \begin{bmatrix}c &s\\-s &c\end{bmatrix} [c−ssc] 箭头朝里朝外,顺时针、逆时针,旋转角的正负 高中物理 磁场方向 右手法则 左手系、右手系 左乘、右乘左乘: 坐标系不动,点动,则左乘。
右乘: 点不动,坐标系动,则右乘。 【可以说,如果一个旋转矩阵左乘表示逆时针旋转 theta 角,那么将此矩阵右乘的话则表示顺时针旋转 theta 角】 左乘与右乘是可以变换的。也即是说: R l e f t ( θ ) = R r i g h t ( θ ) , R l e f t ⋅ R r i g h t = I R_{left}(\theta)=R_{right}(\theta),R_{left}\cdot R_{right}=I Rleft(θ)=Rright(θ),Rleft⋅Rright=I特殊欧氏群 S E ( n ) SE(n) SE(n) n n n维欧氏变换
特殊正交群 S O ( n ) SO(n) SO(n)旋转矩阵群 相似变换 S i m ( 3 ) Sim(3) Sim(3) S O ( 3 ) SO(3) SO(3)三维空间的旋转 罗德里格斯公式 (Rodrigues’s Formula) R T ( α ) = R − 1 ( α ) = R ( − α ) R^T(\alpha)=R^{-1}(\alpha)=R(-\alpha) RT(α)=R−1(α)=R(−α)sin θ = − sin ( − θ ) = − cos ( θ + 9 0 ∘ ) = cos ( θ − 9 0 ∘ ) \sin \theta=-\sin (-\theta)=-\cos(\theta+90^\circ)=\cos(\theta-90^\circ) sinθ=−sin(−θ)=−cos(θ+90∘)=cos(θ−90∘)
cos θ = cos ( − θ ) = sin ( θ + 9 0 ∘ ) = − sin ( θ − 9 0 ∘ ) \cos \theta=\cos (-\theta)=\sin(\theta+90^\circ)=-\sin(\theta-90^\circ) cosθ=cos(−θ)=sin(θ+90∘)=−sin(θ−90∘)sin ( θ 1 + θ 2 ) = s 1 c 2 + c 1 s 2 = s 12 \sin(\theta_1+\theta_2)=s_1c_2+c_1s_2=s_{12} sin(θ1+θ2)=s1c2+c1s2=s12
sin ( θ 1 − θ 2 ) = s 1 c 2 − c 1 s 2 \sin(\theta_1-\theta_2)=s_1c_2-c_1s_2 sin(θ1−θ2)=s1c2−c1s2 cos ( θ 1 + θ 2 ) = c 1 c 2 − s 1 s 2 = c 12 \cos(\theta_1+\theta_2)=c_1c_2-s_1s_2=c_{12} cos(θ1+θ2)=c1c2−s1s2=c12 cos ( θ 1 − θ 2 ) = c 1 c 2 + s 1 s 2 \cos(\theta_1-\theta_2)=c_1c_2+s_1s_2 cos(θ1−θ2)=c1c2+s1s2R ( θ ) = [ cos θ − sin θ sin θ cos θ ] = cos θ [ 1 0 0 1 ] + sin θ [ 0 − 1 1 0 ] = exp ( θ [ 0 − 1 1 0 ] ) R(\theta)={\begin{bmatrix}\cos \theta &-\sin \theta \\\sin \theta &\cos \theta \\\end{bmatrix}}=\cos \theta \begin{bmatrix}1 &0\\0 &1\\\end{bmatrix}+\sin \theta \begin{bmatrix}0 &-1\\1 &0\\\end{bmatrix}=\exp(\theta \begin{bmatrix}0 &-1\\1 &0\\\end{bmatrix}) R(θ)=[cosθsinθ−sinθcosθ]=cosθ[1001]+sinθ[01−10]=exp(θ[01−10])
R x ( θ x ) = [ 1 0 0 0 cos θ x − sin θ x 0 sin θ x cos θ x ] = exp ( [ 0 0 0 0 0 − θ x 0 θ x 0 ] ) = r o l l R_{x}(\theta_x)={\begin{bmatrix}1 &0 &0\\0 &\cos \theta_x &-\sin \theta_x \\0 &\sin \theta_x &\cos \theta_x \\\end{bmatrix}}=\exp(\begin{bmatrix}0 &0 &0\\0 &0 &-\theta_x\\0 &\theta_x &0\end{bmatrix})=roll Rx(θx)=⎣⎡1000cosθxsinθx0−sinθxcosθx⎦⎤=exp(⎣⎡00000θx0−θx0⎦⎤)=roll
R y ( θ y ) = [ cos θ y 0 sin θ y 0 1 0 − sin θ y 0 cos θ y ] = exp ( [ 0 0 θ y 0 0 0 − θ y 0 0 ] ) = p i t c h R_{y}(\theta_y )={\begin{bmatrix}\cos \theta_y &0&\sin \theta_y \\0&1&0\\-\sin \theta_y &0&\cos \theta_y \\\end{bmatrix}}=\exp(\begin{bmatrix}0 &0 &\theta_y\\0 &0 &0\\-\theta_y &0 &0\end{bmatrix})=pitch Ry(θy)=⎣⎡cosθy0−sinθy010sinθy0cosθy⎦⎤=exp(⎣⎡00−θy000θy00⎦⎤)=pitch
R z ( θ z ) = [ cos θ z − sin θ z 0 sin θ z cos θ z 0 0 0 1 ] = exp ( [ 0 − θ z 0 θ z 0 0 0 0 0 ] ) = y a w R_{z}(\theta_z )={\begin{bmatrix}\cos \theta_z &-\sin \theta_z &0\\\sin \theta_z &\cos \theta_z &0\\0&0&1\\\end{bmatrix}}=\exp(\begin{bmatrix}0 &-\theta_z &0\\\theta_z &0 &0\\0 &0 &0\end{bmatrix})=yaw Rz(θz)=⎣⎡cosθzsinθz0−sinθzcosθz0001⎦⎤=exp(⎣⎡0θz0−θz00000⎦⎤)=yaw
M = R z ( θ z ) R y ( θ y ) R x ( θ x ) = exp ( [ 0 − θ z θ y θ z 0 − θ x − θ y θ x 0 ] ) M=R_z(\theta_z)R_y(\theta_y)R_x(\theta_x)=\exp(\begin{bmatrix}0 &-\theta_z &\theta_y\\\theta_z &0 &-\theta_x\\-\theta_y &\theta_x &0\end{bmatrix}) M=Rz(θz)Ry(θy)Rx(θx)=exp(⎣⎡0θz−θy−θz0θxθy−θx0⎦⎤)
eul = [0 pi/2 0]; % z y x
rotmZYX = eul2rotm(eul)M 0 = [ 1 − θ z θ y θ z 1 − θ x − θ y θ x 1 ] M_0=\begin{bmatrix}1 &-\theta_z &\theta_y\\\theta_z &1 &-\theta_x\\-\theta_y &\theta_x &1\end{bmatrix} M0=⎣⎡1θz−θy−θz1θxθy−θx1⎦⎤
1.绕物体的Z 轴旋转, 得到偏航角yaw;
2.绕旋转之后 的Y 轴旋转, 得到俯仰角pitch; 3.绕旋转之后 的X 轴旋转, 得到滚转角roll。Local east, north, up (ENU) coordinates
R z ( + ( 90 + L ) ) R x ( + ( 90 − B ) ) R_z(+(90+L))R_x(+(90-B)) Rz(+(90+L))Rx(+(90−B)) 逆 R x ( − ( 90 − B ) ) R z ( − ( 90 + L ) ) R_x(-(90-B))R_z(-(90+L)) Rx(−(90−B))Rz(−(90+L)) 正 右乘R = [ − sin λ − sin ϕ cos λ cos ϕ cos λ cos λ − sin ϕ sin λ cos ϕ sin λ 0 cos ϕ sin ϕ ] {\displaystyle R={\begin{bmatrix}-\sin \lambda &-\sin \phi \cos \lambda &\cos \phi \cos \lambda \\\cos \lambda &-\sin \phi \sin \lambda &\cos \phi \sin \lambda \\0&\cos \phi &\sin \phi \end{bmatrix}}} R=⎣⎡−sinλcosλ0−sinϕcosλ−sinϕsinλcosϕcosϕcosλcosϕsinλsinϕ⎦⎤
Local north, east, down (NED) coordinates
R z ( + L ) R y ( − ( 90 + B ) ) R_z(+L)R_y(-(90+B)) Rz(+L)Ry(−(90+B)) 逆 R y ( + ( 90 + B ) ) R z ( − L ) R_y(+(90+B))R_z(-L) Ry(+(90+B))Rz(−L) 正 右乘R = [ − sin ( ϕ ) cos ( λ ) − sin ( λ ) − cos ( ϕ ) cos ( λ ) − sin ( ϕ ) sin ( λ ) cos ( λ ) − cos ( ϕ ) sin ( λ ) cos ( ϕ ) 0 − sin ( ϕ ) ] {\displaystyle R={\begin{bmatrix}-\sin(\phi )\cos(\lambda )&-\sin(\lambda )&-\cos(\phi )\cos(\lambda )\\-\sin(\phi )\sin(\lambda )&\cos(\lambda )&-\cos(\phi )\sin(\lambda )\\\cos(\phi )&0&-\sin(\phi )\end{bmatrix}}} R=⎣⎡−sin(ϕ)cos(λ)−sin(ϕ)sin(λ)cos(ϕ)−sin(λ)cos(λ)0−cos(ϕ)cos(λ)−cos(ϕ)sin(λ)−sin(ϕ)⎦⎤
3-D Coordinate and Vector Transformations
geodetic2enu enu2geodeticBLH2XYZ
Geodetic coordinates (latitude ϕ {\displaystyle\ \phi } ϕ, longitude λ {\displaystyle \ \lambda } λ, height h {\displaystyle h} h) can be converted into ECEF coordinates using the following equation:X = ( N ( ϕ ) + h ) cos ϕ cos λ Y = ( N ( ϕ ) + h ) cos ϕ sin λ Z = ( b 2 a 2 N ( ϕ ) + h ) sin ϕ {\displaystyle {\begin{aligned}X&=\left(N(\phi )+h\right)\cos {\phi }\cos {\lambda }\\Y&=\left(N(\phi )+h\right)\cos {\phi }\sin {\lambda }\\Z&=\left({\frac {b^{2}}{a^{2}}}N(\phi )+h\right)\sin {\phi }\end{aligned}}} XYZ=(N(ϕ)+h)cosϕcosλ=(N(ϕ)+h)cosϕsinλ=(a2b2N(ϕ)+h)sinϕ
where N ( ϕ ) = a 2 a 2 cos 2 ϕ + b 2 sin 2 ϕ = a 1 − e 2 sin 2 ϕ , {\displaystyle N(\phi )={\frac {a^{2}}{\sqrt {a^{2}\cos ^{2}\phi +b^{2}\sin ^{2}\phi }}}={\frac {a}{\sqrt {1-e^{2}\sin ^{2}\phi }}},} N(ϕ)=a2cos2ϕ+b2sin2ϕa2=1−e2sin2ϕa, 其中 e 2 = 1 − b 2 a 2 {\displaystyle e^{2}=1-{\frac {b^{2}}{a^{2}}}} e2=1−a2b2{ B = arctan [ z e + ( e ′ ) 2 ⋅ b ⋅ sin 3 U x e 2 + y e 2 − a ⋅ e 2 ⋅ cos 3 U ] L = arctan y e x e H = x e 2 + y e 2 cos B − N \left\{\begin{array}{l}B=\arctan \left[\frac{z_{e}+\left(e^{\prime}\right)^{2} \cdot b \cdot \sin ^{3} U}{\sqrt{x_{e}^{2}+y_{e}^{2}}-a \cdot e^{2} \cdot \cos ^{3} U}\right]\\\\L=\arctan \frac{y_e}{x_e}\\\\H=\frac{\sqrt{x_{e}^{2}+y_{e}^{2}}}{\cos B}-N\\\end{array}\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧B=arctan[xe2+ye2−a⋅e2⋅cos3Uze+(e′)2⋅b⋅sin3U]L=arctanxeyeH=cosBxe2+ye2−N
当大地高 H < 1000 k m H<1000km H<1000km 时,上式的计算精度可达厘米级。但当大地高过大时,纬度计算精度将下降,且大地高H的计算稳定性也会下降。为此 Bowring于1985年又给出下列改进公式:
[ bowring1985 ] THE ACCURACY OF GEODETIC LATITUDE AND HEIGHT EQUATIONS
用上式计算时纬度的精度可达 1 ′ ′ × 1 0 − 7 1''\times10^{-7} 1′′×10−7 ,大地高的误差小于 1 0 − 6 c m 10^{-6}cm 10−6cm ,可满足
各种用户的要求。注意上式主要用于高精度的大地测量计算等应用场合,而对于导航来讲之前的公式一般即可满足要求。Δ lat 1 = π a ( 1 − e 2 ) 18 0 ∘ ( 1 − e 2 sin 2 ϕ ) 3 2 {\displaystyle \Delta _{\text{lat}}^{1}={\frac {\pi a\left(1-e^{2}\right)}{180^{\circ }\left(1-e^{2}\sin ^{2}\phi \right)^{\frac {3}{2}}}}} Δlat1=180∘(1−e2sin2ϕ)23πa(1−e2)
WGS84 spheroid
Δ lat 1 = 111 132.954 − 559.822 cos 2 ϕ + 1.175 cos 4 ϕ {\displaystyle \Delta _{\text{lat}}^{1}=111\,132.954-559.822\cos 2\phi +1.175\cos 4\phi } Δlat1=111132.954−559.822cos2ϕ+1.175cos4ϕ
Δ long 1 = π a cos ϕ 18 0 ∘ 1 − e 2 sin 2 ϕ {\displaystyle \Delta _{\text{long}}^{1}={\frac {\pi a\cos \phi }{180^{\circ }{\sqrt {1-e^{2}\sin ^{2}\phi }}}}\,} Δlong1=180∘1−e2sin2ϕπacosϕ
ϕ \phi ϕ | Δ lat 1 \Delta _{\text{lat}}^{1} Δlat1 | Δ long 1 \Delta _{\text{long}}^{1} Δlong1 |
---|---|---|
0° | 110.574 km | 111.320 km |
15° | 110.649 km | 107.550 km |
30° | 110.852 km | 96.486 km |
45° | 111.132 km | 78.847 km |
60° | 111.412 km | 55.800 km |
75° | 111.618 km | 28.902 km |
90° | 111.694 km | 0.000 km |
( d X d Y d Z ) = ( − sin λ − sin ϕ cos λ cos ϕ cos λ cos λ − sin ϕ sin λ cos ϕ sin λ 0 cos ϕ sin ϕ ) ( d E d N d U ) , ( d E d N d U ) = ( ( N ( ϕ ) + h ) cos ϕ 0 0 0 M ( ϕ ) + h 0 0 0 1 ) ( d λ d ϕ d h ) , {\displaystyle {\begin{aligned}{\begin{pmatrix}dX\\dY\\dZ\end{pmatrix}}&={\begin{pmatrix}-\sin \lambda &-\sin \phi \cos \lambda &\cos \phi \cos \lambda \\\cos \lambda &-\sin \phi \sin \lambda &\cos \phi \sin \lambda \\0&\cos \phi &\sin \phi \\\end{pmatrix}}{\begin{pmatrix}dE\\dN\\dU\end{pmatrix}},\\[3pt]{\begin{pmatrix}dE\\dN\\dU\end{pmatrix}}&={\begin{pmatrix}\left(N(\phi )+h\right)\cos \phi &0&0\\0&M(\phi )+h&0\\0&0&1\\\end{pmatrix}}{\begin{pmatrix}d\lambda \\d\phi \\dh\end{pmatrix}},\end{aligned}}} ⎝⎛dXdYdZ⎠⎞⎝⎛dEdNdU⎠⎞=⎝⎛−sinλcosλ0−sinϕcosλ−sinϕsinλcosϕcosϕcosλcosϕsinλsinϕ⎠⎞⎝⎛dEdNdU⎠⎞,=⎝⎛(N(ϕ)+h)cosϕ000M(ϕ)+h0001⎠⎞⎝⎛dλdϕdh⎠⎞,
M ( ϕ ) = a ( 1 − e 2 ) ( 1 − e 2 sin 2 ϕ ) 3 2 {\displaystyle M(\phi )={\frac {a\left(1-e^{2}\right)}{\left(1-e^{2}\sin ^{2}\phi \right)^{\frac {3}{2}}}}} M(ϕ)=(1−e2sin2ϕ)23a(1−e2)
[ x y z ] = [ E N U ] \begin{bmatrix}x\\y\\z\end{bmatrix}=\begin{bmatrix}E\\N\\U\end{bmatrix} ⎣⎡xyz⎦⎤=⎣⎡ENU⎦⎤
大地坐标系(φ,λ,h)纬度、经度、高度,geodetic
地心地固坐标系(Earth-Centered, Earth-Fixed,简称ECEF)简称地心坐标系 WGS-84椭球模型先绕z轴正向 逆时针 旋转 90+L, 再绕x轴 顺时针 旋转 90-B. -号表示为右乘:点不动,坐标系动
R x ( − ( 90 − B ) ) R z ( − ( 90 + L ) ) = [ − sin L cos L 0 − sin B cos L − sin B sin L cos B cos B cos L cos B sin L sin B ] R_x(-(90-B))R_z(-(90+L))={\begin{bmatrix}-\sin L&\cos L&0\\-\sin B\cos L&-\sin B\sin L&\cos B\\\cos B\cos L&\cos B\sin L&\sin B\end{bmatrix}} Rx(−(90−B))Rz(−(90+L))=⎣⎡−sinL−sinBcosLcosBcosLcosL−sinBsinLcosBsinL0cosBsinB⎦⎤ 正 右乘R x ( − ( 90 − B ) ) = [ 1 0 0 0 cos ( B − 90 ) − sin ( B − 90 ) 0 sin ( B − 90 ) cos ( B − 90 ) ] = [ 1 0 0 0 sin B cos B 0 − cos B sin B ] {\displaystyle R_x(-(90-B))= {\begin{bmatrix}1 &0 &0\\0 &\cos(B-90) &-\sin(B-90) \\0 &\sin(B-90) &\cos(B-90)\end{bmatrix}}= {\begin{bmatrix}1 &0 &0\\0 &\sin B &\cos B \\0 &-\cos B &\sin B\end{bmatrix}}} Rx(−(90−B))=⎣⎡1000cos(B−90)sin(B−90)0−sin(B−90)cos(B−90)⎦⎤=⎣⎡1000sinB−cosB0cosBsinB⎦⎤
R z ( − ( 90 + L ) ) = [ cos ( L + 90 ) sin ( L + 90 ) 0 − sin ( L + 90 ) cos ( L + 90 ) 0 0 0 1 ] = [ − sin L cos L 0 − cos L − sin L 0 0 0 1 ] {\displaystyle R_z(-(90+L))={\begin{bmatrix}\cos(L+90) &\sin(L+90) &0 \\ -\sin(L+90) &\cos(L+90) &0\\ 0 &0 &1\end{bmatrix}}={\begin{bmatrix}-\sin L &\cos L &0 \\ -\cos L &-\sin L &0\\ 0 &0 &1\end{bmatrix}}} Rz(−(90+L))=⎣⎡cos(L+90)−sin(L+90)0sin(L+90)cos(L+90)0001⎦⎤=⎣⎡−sinL−cosL0cosL−sinL0001⎦⎤
[ x y z ] = [ − sin λ r cos λ r 0 − sin ϕ r cos λ r − sin ϕ r sin λ r cos ϕ r cos ϕ r cos λ r cos ϕ r sin λ r sin ϕ r ] [ X p − X r Y p − Y r Z p − Z r ] {\displaystyle {\begin{bmatrix}x\\y\\z\end{bmatrix}}={\begin{bmatrix}-\sin \lambda _{r}&\cos \lambda _{r}&0\\-\sin \phi _{r}\cos \lambda _{r}&-\sin \phi _{r}\sin \lambda _{r}&\cos \phi _{r}\\\cos \phi _{r}\cos \lambda _{r}&\cos \phi _{r}\sin \lambda _{r}&\sin \phi _{r}\end{bmatrix}}{\begin{bmatrix}X_{p}-X_{r}\\Y_{p}-Y_{r}\\Z_{p}-Z_{r}\end{bmatrix}}} ⎣⎡xyz⎦⎤=⎣⎡−sinλr−sinϕrcosλrcosϕrcosλrcosλr−sinϕrsinλrcosϕrsinλr0cosϕrsinϕr⎦⎤⎣⎡Xp−XrYp−YrZp−Zr⎦⎤
R z ( + ( 90 + L ) ) R x ( + ( 90 − B ) ) R_z(+(90+L))R_x(+(90-B)) Rz(+(90+L))Rx(+(90−B)) 逆
[ X Y Z ] = [ − sin λ − sin ϕ cos λ cos ϕ cos λ cos λ − sin ϕ sin λ cos ϕ sin λ 0 cos ϕ sin ϕ ] [ x y z ] + [ X r Y r Z r ] {\displaystyle {\begin{bmatrix}X\\Y\\Z\end{bmatrix}}={\begin{bmatrix}-\sin \lambda &-\sin \phi \cos \lambda &\cos \phi \cos \lambda \\\cos \lambda &-\sin \phi \sin \lambda &\cos \phi \sin \lambda \\0&\cos \phi &\sin \phi \end{bmatrix}}{\begin{bmatrix}x\\y\\z\end{bmatrix}}+{\begin{bmatrix}X_{r}\\Y_{r}\\Z_{r}\end{bmatrix}}} ⎣⎡XYZ⎦⎤=⎣⎡−sinλcosλ0−sinϕcosλ−sinϕsinλcosϕcosϕcosλcosϕsinλsinϕ⎦⎤⎣⎡xyz⎦⎤+⎣⎡XrYrZr⎦⎤可根据BLH、DAE求BLH,
[lat,lon,h] = aer2geodetic(az,elev,slantRange,lat0,lon0,h0,spheroid)
如果你觉得以上内容对你有帮助,欢迎赞赏、转发!
转载地址:http://ekcqi.baihongyu.com/