DVL+INS_MATLAB2018
理解DVL/INS标定过程
Inertial Navigation System (INS) 和 Doppler Velocity Log (DVL) 组合导航系统标定是一个将这两种导航技术融合在一起以获得更高精度和更长时间的定位结果的过程。
一般来说,INS 根据加速度传感器和角速度传感器的数据算出姿态和位置。而 DVL 则通过分析地面(海底、对底速度)反射的声波信号,计算出相对于地面的速度。
在 INS/DVL 组合导航系统标定过程中,需要对每一个传感器的误差进行标定。这通常包括确定加速度传感器的偏差和角速度传感器的误差,以及计算 DVL 的每一个轴的误差。
标定过程通常需要一些参考数据,如 GPS 定位数据或基准地面数据,以确定传感器误差的程度。标定后,这些误差数据可以用于在实际使用中修正 INS 和 DVL 的测量结果。
标定过程可以用数学方法表示为一个最小二乘优化问题,其目标是最小化误差的平方和。设误差为
ϵ
\epsilon
ϵ,则最小二乘问题可以表示为:
min
x
1
2
ϵ
T
ϵ
\min\limits_{x} \frac{1}{2} \epsilon^{T} \epsilon
xmin21ϵTϵ
其中,
x
x
x 表示待标定的参数,
ϵ
\epsilon
ϵ 表
% 使用卡尔曼滤波器融合DVL、INS和USBL数据
% 假设已经获得了DVL、INS和USBL的数据
% 定义卡尔曼滤波器
% 先验估计值为0,协方差矩阵为1
x = [0;0;0]; % 初始状态向量
P = [1 0 0;0 1 0;0 0 1]; % 初始协方差矩阵
% 观测矩阵
H = [1 0 0;0 1 0;0 0 1]; % 对速度、位置和位置偏差均进行观测
% 观测噪声协方差矩阵
R = [0.1 0 0;0 0.1 0;0 0 0.1]; % 假设观测噪声很小
% 状态转移矩阵
F = [1 1 0;0 1 0;0 0 1]; % 假设速度不变,位置随时间变化,位置偏差不变
% 状态转移噪声协方差矩阵
Q = [0.1 0 0;0 0.1 0;0 0 0.1]; % 假设状态转移噪声很小
% 迭代计算卡尔曼滤波器
for i = 1:length(time)
% 预测步
x_
这里应该是没写完👆👆👆👆
Kalman Filter _MATLAB2018
% 初始状态的估计
xhat = [0;0];
P = [1 0;0 1];
% 测量噪声的协方差
R = 0.1;
% 状态转移矩阵
F = [1 1;0 1];
% 输入噪声的协方差
Q = [0.05 0;0 0.05];
% 测量更新
for i = 1:100
% 预测
xhat = F*xhat;
P = F*P*F' + Q;
% 测量更新
z = sin(i/10) + randn*R;
y = z - xhat(1);
S = P(1,1) + R;
K = P*[1;0]/S;
xhat = xhat + K*y;
P = P - K*[1;0]'*P;
end
=
Gamma公式展示
Γ
(
n
)
=
(
n
−
1
)
!
∀
n
∈
N
\Gamma(n) = (n-1)!\quad\forall n\in\mathbb N
Γ(n)=(n−1)!∀n∈N 是通过 Euler integral
Γ ( z ) = ∫ 0 ∞ t z − 1 e − t d t . \Gamma(z) = \int_0^\infty t^{z-1}e^{-t}dt\,. Γ(z)=∫0∞tz−1e−tdt.