matlab 罗德里格斯变换,修正罗德里格斯参数
修正Rodrigues参数（modified Rodrigues parameters）又被称作MRP。该方法用于描述两个坐标系之间的方向关系，并基于欧拉四元数进行定义。
三个参数组成的向量即为修正罗德里格斯参数\hat \sigma：
性质
与主旋转矢量的关系
定义在主旋转矢量中主要轴为\hat e，对应的角度值记为\Phi。由此可得修正后的罗德里格斯参数\hat q将由上述两个要素共同决定：
在主角度\Phi小于\pi时，可认为：
下图显示了主角度$\Phi
与欧拉四元数的关系
由定义式：
此外，反向关系为：
与经典罗德里格斯参数的关系
设经典罗德里格斯参数为\hat q，则有转化关系：
与方向余弦矩阵的关系
方向余弦矩阵用修正罗德里格斯参数表示为：
其中\left[ I \right]为单位阵。展开式不能写成简单形式，故不在此列出。
且有性质：
基于已知的方向余弦矩阵计算修正罗德里格斯参数，则首先定义中间变量ξ为：
其中\beta_0即属于第一个欧拉四元数参数，在此情境下。
trace\left[ C \right]则表示矩阵\left[ C \right]的迹。
进一步，有：
\[\hat \sigma = \frac{1}{\xi (\xi + 2)}\left( {\begin{array}{*{20}{c}}
{C_{23} - {C_{32}}}\
{C_{31} - {C_{13}}}\
{C_{12} - {C_{21}}}
\end{array}} \right)]
\hat \sigma's cross product matrix \left[ {\tilde \sigma} \right] may also be expressed as a simple combination of the direction cosine matrix.
\left[ {\tilde \sigma} \right]也与凯莱变换有密切关系。
与角速度的关系
基于欧拉四元数所建立的微分方程体系，并结合四元数与修正罗达格斯参数间的内在联系，在经过较为繁琐而细致的演算推导后终于获得了更为简洁直观地描述修正罗达格斯参数的时间变化规律及其与角速度之间相互依存的关系式：
或者写为展开形式：
\[\dot \sigma = \frac{1}{4}\left[ {\begin{array}{*{20}{c}}
{1 - {\sigma ^2} + 2\sigma _1^2}&{2({\sigma _1}{\sigma _2} - {\sigma _3})}&{2({\sigma _1}{\sigma _3} + {\sigma _2})}\
{2({\sigma _2}{\sigma _1} + {\sigma _3})}&{1 - {\sigma ^2} + 2\sigma _2^2}&{2({\sigma _2}{\sigma _3} - {\sigma _1})}\
{2({\sigma _3}{\sigma _1} - {\sigma _2})}&{2({\sigma _3}{\sigma _2} + {\sigma _1})}&{1 - {\sigma ^2} + 2\sigma _3^2}
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{\omega _1}\
{\omega _2}\
{\omega _3}
\end{array}} \right]]
特别地，当主角度\Phi为0时，[B(\hat \sigma )]为单位阵。
反向转换，有：
[B(\hat \sigma )]是近似正交的，即：
从而有：
奇点
基于修正罗德里格斯参数的定义中可以看出，在取\beta_0 = -1的情况下会出现分母为零的问题而导致奇点现象出现。此时对应的主角度值应满足\Phi = \pm 2\pi。值得注意的是，在这种情况下（即正转或反转360度均与无旋转的效果相同），我们无需进行特殊处理和考虑。而在实际应用中通常会采用辅助影集的方法来规避这一问题（后续内容将详细说明）。
不可叠加性
这里的不可叠加性体现为两次旋转的修正罗德里格斯参数之和不等于等效的修正罗德里格斯参数。然而，在处理这一问题时，我们通常会将修正罗德里格斯参数转化为方向余弦矩阵进行处理，并通过方向余弦矩阵进行叠加运算以实现叠加效果。最终，在总的方向余弦矩阵中确定相应的等效修正罗德里格斯参数。具体而言，在实际应用中可以选择坐标系N到B的转换过程作为示例研究对象，并将其分解为两个连续的过程：即坐标系N到F的转换与F到B的具体变换操作联合表达即可完成整个转换关系描述。
最终可得等效的修正罗德里格斯参数为：
也可写为另一种形式：
通过比较，在减法形式与加法形式的对比中，在数学表达上仅通过以下两个途径实现差异：交换位置，并对其中一个取反符号。
特别关注在转换过程中分母可能出现的奇点情况。当这种情况发生时，则建议将参与运算的两个修正Rodrigues参数中的一个替换为其对应的伴随集合（后续会有详细说明）。
几何意义
基于欧拉四元数所遵循的约束关系可知，在将之视为四维空间坐标时，则其运动轨迹始终位于一个单位超球体内而不超出此范围。仅关注与\beta_0及\beta_i（其中i=1,2,3）相关的二维投影平面时，则该四元数在其对应的二维投影平面内必处于该平面内的单位圆内部位置。
由修正罗德里格斯参数的定义式：
由此可知，在y轴方向上进行分析时（即考虑quaternion projection），\sigma_i等于quaternion projection与点(-1, 0)之间的连线所形成的截距。每一个二维平面投影都是如此，则当考虑quaternion及其对应的高维空间中的连线时（即连接点(-1, 0, 0, 0)），\hat{\sigma}即为该连线与超平面\beta_0 = 0的交点。
而四元数与原点所连之线与满足\beta_i=0（其中i=1,2,3）的所有点所构成的空间形成的角度即是主角度\frac{\Phi}{2}。其情形如下图所示
伴影集
定义
修正罗德里格斯参数的伴影集（shadow set）旨在维持其与主旋转角Φ之间的近似线性关系。
也可用主轴\hat e和主角度\Phi来表示：
修正罗德里格斯参数及其伴影集通常位于超球体内与体外，并在特殊情况下位于超球面上。图中清晰展示了这一特殊情况。当β₀=0时，在超球体上的σ_i与σ_is分别为正负值。值得注意的是，在原点处的σ_i对应于无穷远处的σ_is。
校正罗德里格斯参数并非与姿态一一对应。而该修正后的参数及其伴影集则代表同一个姿态。
应用
因为优化罗德里格斯参数及其伴随影集实际上代表了同一姿态的不同数学表述方式，在两者之间进行转换操作并不会对问题陈述的性质产生任何影响。为了避免出现解算过程中可能出现的奇异点，并且精确调节主角度Φ的值域范围。
应用与评价
修正罗德里格斯参数通过三个参数表征了三维坐标系中的旋转运动，在此过程中其独特的优势体现在：能够有效避免奇点的发生；与其他常用的旋转表示方法相比具有较低的计算复杂度；而其微分方程的形式则更为简洁易懂。由此可见，在工程实践中该方法的应用前景十分广阔。对于求解不同坐标系间的变换关系，在数值计算和工程优化中该方法往往被采用。
附录：Matlab程序
以下Matlab程序均来自
转换为方向余弦矩阵
function C = MRP2C(q)
% MRP2C
%
%C = MRP2C(Q) returns the direction cosine
%matrix in terms of the 3x1 MRP vector Q.
%
q1 = q(1);
q2 = q(2);
q3 = q(3);
d1 = q'*q;
S = 1-d1;
d = (1+d1)*(1+d1);
C(1,1) = 4*(2q1q1-d1)+S*S;
C(1,2) = 8q1q2+4q3S;
C(1,3) = 8q1q3-4q2S;
C(2,1) = 8q2q1-4q3S;
C(2,2) = 4*(2q2q2-d1)+S*S;
C(2,3) = 8q2q3+4q1S;
C(3,1) = 8q3q1+4q2S;
C(3,2) = 8q3q2-4q1S;
C(3,3) = 4*(2q3q3-d1)+S*S;
C = C/d;
转换为欧拉四元数
functionq = MRP2EP(q1)
% MRP2EP(Q1)
%
%Q = MRP2EP(Q1) translates the MRP vector Q1
%into the Euler parameter vector Q.
%
ps = 1+q1'*q1;
q(1) = (1-q1'*q1)/ps;
q(2) = 2*q1(1)/ps;
q(3) = 2*q1(2)/ps;
q(4) = 2*q1(3)/ps;
q=q';
转换为吉布斯矢量
functionq = MRP2Gibbs(q1)
% MRP2Gibbs(Q1)
%
%Q = MRP2Gibbs(Q1) translates the MRP vector Q1
%into the Gibbs vector Q.
%
q = 2*q1/(1-q1'*q1);
转换为主旋转矢量
functionq = MRP2PRV(q1)
% MRP2PRV(Q1)
%
%Q = MRP2PRV(Q1) translates the MRP vector Q1
%into the principal rotation vector Q.
%
tp = sqrt(q1'*q1);
p = 4*atan(tp);
q(1) = q1(1)/tp*p;
q(2) = q1(2)/tp*p;
q(3) = q1(3)/tp*p;
q=q';
转换为伴影集
function s = MRPswitch(q,s2)
% MRPswitch
%
%S = MRPswitch(Q,s2) checks to see if norm(Q) is larger than s2.
%If yes, then the MRP vector Q is mapped to its shadow set.
%
q2 = q'*q;
if (q2>s2*s2)
s = -q/q2;
else
s = q;
end
微分方程
下面的程序用来求[B]矩阵。它的定义是：
\[\dot \sigma = \frac{1}{4}\left[ B \right]\left[ {\begin{array}{*{20}{c}}
{ {\omega _1} }\
{ {\omega _2} }\
{ {\omega _3} }
\end{array}} \right]]
function B = BmatMRP(q)
% BmatMRP(Q)
%
%B = BmatMRP(Q) returns the 3x3 matrix which relates the
%body angular velocity vector w to the derivative of
%MRP vector Q.
%
%dQ/dt = 1/4 [B(Q)] w
%
s2 = q'*q;
B(1,1) = 1-s2+2*q(1)*q(1);
B(1,2) = 2*(q(1)*q(2)-q(3));
B(1,3) = 2*(q(1)*q(3)+q(2));
B(2,1) = 2*(q(2)*q(1)+q(3));
B(2,2) = 1-s2+2*q(2)*q(2);
B(2,3) = 2*(q(2)*q(3)-q(1));
B(3,1) = 2*(q(3)*q(1)-q(2));
B(3,2) = 2*(q(3)*q(2)+q(1));
B(3,3) = 1-s2+2*q(3)*q(3);
或者反过来：
\[\left[ {\begin{array}{*{20}{c}}
{ {\omega _1} }\
{ {\omega _2} }\
{ {\omega _3} }
\end{array}} \right] = 4{\left[ B \right]^{ - 1}}\dot \sigma ]
下面的程序用来求[B]的逆矩阵。
function B = BinvMRP(q)
% BinvMRP(Q)
%
%B = BinvMRP(Q) returns the 3x3 matrix which relates
%the derivative of MRP vector Q to the
%body angular velocity vector w.
%
%w = 4 [B(Q)]^(-1) dQ/dt
%
s2 = q'*q;
B(1,1) = 1-s2+2*q(1)*q(1);
B(1,2) = 2*(q(1)*q(2)+q(3));
B(1,3) = 2*(q(1)*q(3)-q(2));
B(2,1) = 2*(q(2)*q(1)-q(3));
B(2,2) = 1-s2+2*q(2)*q(2);
B(2,3) = 2*(q(2)*q(3)+q(1));
B(3,1) = 2*(q(3)*q(1)+q(2));
B(3,2) = 2*(q(3)*q(2)-q(1));
B(3,3) = 1-s2+2*q(3)*q(3);
B = B/(1+s2)/(1+s2);
H. Schaub和J.L. Junkins合著的《空间动力学分析》一书为第二版（2nd edition），属于AIAA教育系列出版物（AIAA Education Series），在2009年出版。