Algorithm for GPS/INS
杆臂效应lever arm effect
在GINS/GPS组合导航系统中，空间不同步是指惯性测量单元（IMU）和全球定位系统（GPS）所处的坐标系之间存在一定的旋转差异，这可能会导致导航误差，称为杆臂效应（lever arm effect）。具体而言，由于IMU和GPS安装位置不同，导致IMU和GPS测量得到的加速度和角速度数据存在一个旋转矩阵，需要对其进行转换，使其在同一坐标系下进行比较。如果这种旋转矩阵没有得到正确的校准，就会导致导航精度降低，从而引起所谓的杆臂效应。
杆臂效应可以用以下公式表示：
δ
p
=
C
b
n
∗
δ
p
b
+
V
∗
δ
t
δp = C_b^n * δp_b + V * δt
δp=Cbn∗δpb+V∗δt
其中，
δ
p
δp
δp是导航误差，
C
b
n
C_b^n
Cbn是从机体坐标系到导航坐标系的旋转矩阵，
δ
p
b
δp_b
δpb是惯性测量单元测量得到的加速度和角速度误差，
V
V
V是机体速度，
δ
t
δt
δt是时间误差。可以看出，由于
C
b
n
C_b^n
Cbn矩阵的误差，会导致
δ
p
δp
δp误差的产生，进而影响导航精度。
为了解决杆臂效应，需要进行空间不同步校准，即确定IMU和GPS安装位置之间的精确旋转矩阵，从而消除旋转差异带来的误差。常见的校准方法有静态标定法和动态标定法。静态标定法是在静止状态下，通过测量IMU和GPS所处的旋转矩阵，以确定其精确的旋转差异；而动态标定法则是在运动状态下，通过测量GPS信号和IMU输出的数据，以动态地校准IMU和GPS之间的空间不同步。
总之，杆臂效应是GINS/GPS组合导航系统中常见的误差源，需要通过空间不同步校准来消除其对导航精度的影响。
杆臂效应补偿
SINS/GPS组合导航中，由于惯性测量单元（IMU）和GPS接收机之间存在空间不同步，会产生杆臂效应，从而影响导航精度。常见的SINS/GPS组合导航杆臂效应的补偿算法包括以下几种：
-
误差状态扩展卡尔曼滤波（ES-EKF）算法：该算法将杆臂误差作为一个附加状态引入到扩展卡尔曼滤波器中，通过卡尔曼滤波来估计和补偿杆臂误差。
-
双正交矩阵法：该方法通过对姿态角速度和GPS速度的观测数据进行双正交矩阵分解，来减小杆臂误差对导航的影响。
-
均值数学模型法：该方法基于导航模型中对杆臂误差的定义，将杆臂误差建模为两个随机变量的乘积，并使用卡尔曼滤波算法进行估计和补偿。
-
优化算法：该算法通过对杆臂误差进行优化，来最小化杆臂误差对导航的影响。
总之，SINS/GPS组合导航杆臂效应的补偿算法包括ES-EKF算法、双正交矩阵法、均值数学模型法和优化算法等。这些算法的实现可以通过软件或硬件方式来实现。选择适合自己需求的算法，并根据实际情况进行调试和优化，可以提高导航的精度和可靠性。
严恭敏工具箱
严恭敏PSINS工具箱中关于SINS/GPS组合导航杆臂效应的代码是“CAligni0”和“CAlignkf”。其中，“CAligni0”是基于初始对准的静态对准算法，用于测量IMU和GPS之间的杆臂向量，并提供初始对准姿态矩阵，以便后续的动态对准和导航解算。而“CAlignkf”则是基于扩展卡尔曼滤波的动态对准算法，用于在线估计和补偿杆臂误差，以提高导航精度。
这些代码在PSINS工具箱中的路径是“PSINS/PSINS.cpp”和“PSINS/CAligni0.cpp”。如果您想使用这些代码，可以从严恭敏PSINS工具箱的官方网站上下载并安装该工具箱。下载地址为：http://www.chunzhounan.com/psins.html。工具箱地址安装完成后，您可以打开工具箱的示例程序，查看如何使用这些代码实现SINS/GPS组合导航中的杆臂效应补偿。
坐标系
地固系（e）
惯性系（i）
当地水平坐标系(l)
（地理坐标系（g）《自主导航技术》胡小平page4
载体坐标系（b）
之间如何转化（论文13页。书5页之后）
GPS/INS
回答1
UUV利用INS/GPS组合导航定位，通常使用卡尔曼滤波算法对GPS和INS数据进行融合。卡尔曼滤波算法的基本原理是通过在状态空间内建立状态估计模型，并通过观察数据和预测模型的协同更新来得到最优的状态估计值。
具体而言，卡尔曼滤波算法的计算公式如下：
预测阶段：
预测状态值：
x
k
=
F
k
−
1
∗
x
k
−
1
x_k = F_k-1 * x_k{-1}
xk=Fk−1∗xk−1
预测误差协方差矩阵：
P
k
=
(
F
k
−
1
∗
P
k
−
1
∗
F
k
−
1
)
T
+
Q
k
P_k = (F_k^{-1} * P_k-1 * F_k^{-1})^T + Q_k
Pk=(Fk−1∗Pk−1∗Fk−1)T+Qk
其中， F k − 1 F_k^{-1} Fk−1是状态转移矩阵， x k − 1 x_k^{-1} xk−1是状态估计值， P k − 1 P_k^{-1} Pk−1 是误差协方差矩阵， Q k Q_k Qk是状态噪声的协方差矩阵。
更新阶段：
计算卡尔曼增益：
K
k
=
P
k
∗
H
k
T
∗
(
H
k
∗
P
k
∗
H
k
T
+
R
k
)
−
1
K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^{-1}
Kk=Pk∗HkT∗(Hk∗Pk∗HkT+Rk)−1
更新状态估计值：
x
k
=
x
k
+
K
k
∗
(
z
k
−
H
k
∗
x
k
)
x_k = x_k + K_k * (z_k - H_k * x_k)
xk=xk+Kk∗(zk−Hk∗xk)
更新误差协方差矩阵：
P
k
=
(
I
−
K
k
∗
H
k
)
∗
P
k
P_k = (I - K_k * H_k) * P_k
Pk=(I−Kk∗Hk)∗Pk
其中，
H
k
H_k
Hk 是观测矩阵，
z
k
z_k
zk 是观测数据，
R
k
R_k
Rk 是观测噪声的协方差矩阵。
通过不断地循环预测和更新
回答2
UUV利用INS/GPS进行组合导航定位时，采用卡尔曼滤波算法对GPS和INS数据进行融合，可以提高定位精度。
卡尔曼滤波算法是一种基于概率的算法，它将系统的状态当作随机变量，利用状态转移方程和观测方程对系统的状态进行估计。
对于UUV的组合导航定位问题，卡尔曼滤波算法需要对INS和GPS数据进行融合。具体来说，首先需要建立状态转移方程，描述UUV在每一时刻的状态，以及状态的协方差矩阵。然后，需要建立观测方程，描述GPS和INS数据如何观测UUV的状态。最后，通过卡尔曼滤波算法，利用状态转移方程和观测方程，对状态的估计值进行更新，从而得到UUV的定位信息。
下面：卡尔曼滤波公式
具体的，卡尔曼滤波算法的公式如下：
预测状态：
x
^
k
∣
k
−
1
=
F
k
−
1
x
^
k
−
1
∣
k
−
1
\hat{x}{k|k-1} = F{k-1} \hat{x}_{k-1|k-1}
x^k∣k−1=Fk−1x^k−1∣k−1
预测协方差矩阵：
P
k
∣
k
−
1
=
F
k
−
1
P
k
−
1
∣
k
−
1
F
k
−
1
T
+
Q
k
−
1
P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}
Pk∣k−1=Fk−1Pk−1∣k−1Fk−1T+Qk−1
观测残差：
未完👆
回答3
基于卡尔曼滤波算法的组合导航系统，需要对GPS和INS两个传感器的测量数据进行同步处理。
对于GPS数据，需要对接收到的星历数据进行处理，得到当前卫星位置和当前测量位置的误差；对于INS数据，需要对加速度和角速度的测量数据进行积分，得到当前姿态和位置。
在融合过程中，卡尔曼滤波算法需要对状态向量进行不断的更新和优化，以提高系统的精度。卡尔曼滤波的更新过程如下：
预测步骤：首先，使用上一时刻的状态向量对当前状态进行预测。//通过对系统状态的预测，得到预测状态的估计值。
更新步骤：然后，使用GPS和INS的测量数据对预测的状态进行校正。
计算测量误差：通过测量数据计算出测量误差，并用测量误差更新预测状态的估计值。
估计步骤：最后，使用卡尔曼滤波的估计方程，更新当前的状态向量。
更新状态：根据测量误差对预测状态进行更新，得到最终的状态估计值。
对于UUV利用INS/GPS进行组合导航定位，卡尔曼滤波算法的状态估计过程可以用公式表示为：
x
k
=
F
k
x
k
−
1
+
B
k
u
k
x_k = F_k x_{k-1} + B_k u_k
xk=Fkxk−1+Bkuk
z k = H k x k + w k z_k = H_k x_k + w_k zk=Hkxk+wk
K k = P k − 1 H k T ( H k P k − 1 H k T + R ) − 1 K_k = P_{k-1} H_k^T (H_k P_{k-1} H_k^T + R)^{-1} Kk=Pk−1HkT(HkPk−1HkT+R)−1
x k = x k − 1 + K k ( z k − H k x k − 1 ) x_k = x_{k-1} + K_k (z_k - H_k x_{k-1}) xk=xk−1+Kk(zk−Hkxk−1)
P
k
=
(
I
−
K
k
H
k
)
P
k
−
1
P_k = (I - K_k H_k) P_{k-1}
Pk=(I−KkHk)Pk−1
未完👆
预测状态：
x
^
k
∣
k
−
1
=
F
k
−
1
x
^
k
−
1
∣
k
−
1
\hat{x}{k|k-1} = F{k-1} \hat{x}_{k-1|k-1}
x^k∣k−1=Fk−1x^k−1∣k−1
预测协方差矩阵：
P
k
∣
k
−
1
=
F
k
−
1
P
k
−
1
∣
k
−
1
F
k
−
1
T
+
Q
k
−
1
P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}
Pk∣k−1=Fk−1Pk−1∣k−1Fk−1T+Qk−1
观测残差：
对数自适应多径补偿
对数自适应多径补偿算法是一种用于修正USBL水下定位过程中多径干扰的算法。该算法通过对接收到的USBL信号进行对数变换，从而补偿多径干扰的影响。
对数自适应多径补偿算法的具体过程如下：
-
对数变换：对接收到的USBL信号进行对数变换，得到对数信号。
-
自适应：通过算法设计，对对数信号进行自适应，得到自适应系数以得到补偿多径干扰的系数。
-
补偿：根据自适应系数，对对数信号进行补偿，得到补偿后的信号。
-
反变换：对补偿后的信号进行反变换，得到修正后的USBL信号。
对数自适应多径补偿算法具有简单、快速等优点，适用于水下定位系统中多径干扰的补偿。但是，该算法可能在信号强度较弱或信号分布不均匀时存在效果不佳的情况。因此，在实际应用中需要结合具体情况进行合理的调整。
算法公式
对于对数自适应多径补偿算法，具体的公式如下：
对数变换：
y ( t ) = 10 log 10 ( x ( t ) ) y(t) = 10\log_{10}(x(t)) y(t)=10log10(x(t))
其中， x ( t ) x(t) x(t) 为原始USBL信号， y ( t ) y(t) y(t) 为对数信号。
- 自适应：
θ ( t ) = θ ( t − 1 ) + μ e ( t ) \theta(t) = \theta(t-1) + \mu e(t) θ(t)=θ(t−1)+μe(t)
其中， θ ( t ) \theta(t) θ(t) 为算法计算得到的自适应系数， μ \mu μ 为自适应步长， e ( t ) e(t) e(t) 为误差。
- 补偿：
z ( t ) = y ( t ) − θ ( t ) z(t) = y(t) - \theta(t) z(t)=y(t)−θ(t)
其中， z ( t ) z(t) z(t) 为补偿后的对数信号。
- 反变换：
x ′ ( t ) = 1 0 z ( t ) / 10 x'(t) = 10^{z(t)/10} x′(t)=10z(t)/10
其中， x ′ ( t ) x'(t) x′(t) 为修正后的USBL信号。
这些公式描述了对数自适应多径补偿算法的基本过程。在实际应用中，可以根据具体情况进行修改以适应不同的水下环境。
船舶捷联惯导陀螺漂移
惯性导航系统包括三轴加速度和陀螺仪，其中进行误差分析时，什么是陀螺漂移
陀螺漂移是惯性导航系统中的一个重要误差源，指的是陀螺仪在长时间使用后输出的角速度存在的持续性偏移。即使在没有旋转的情况下，陀螺仪也会输出一个非零的角速度，这个偏移量就是陀螺漂移。
陀螺漂移的原因是陀螺仪在运行时受到各种因素的影响，如温度变化、机械振动和零点漂移等，这些因素会导致陀螺仪输出的角速度存在一个稳定的偏移量，这个偏移量会随时间的推移而不断积累，从而导致惯性导航系统的误差逐渐增大。
为了解决陀螺漂移的问题，惯性导航系统通常会采用一些校准技术，如零偏校准和温度补偿等，来消除陀螺漂移对系统精度的影响。此外，还可以采用陀螺组合技术，将多个陀螺仪的输出进行组合，以提高系统的精度和稳定性。
752
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
