diff --git a/src/ShipMMG.jl b/src/ShipMMG.jl index b679342..2c507f8 100644 --- a/src/ShipMMG.jl +++ b/src/ShipMMG.jl @@ -11,6 +11,7 @@ using ForwardDiff include("data.jl") export calc_position, ShipData, + ShipDataAdvanced, get_KVLCC2_L7_basic_params, get_KVLCC2_L7_maneuvering_params, get_KVLCC2_L7_params, @@ -27,12 +28,22 @@ export kt_simulate, create_model_for_mcmc_sample_kt include("mmg.jl") +export Mmg3DofBasicParams, + Mmg3DofManeuveringParams, + mmg_3dof_simulate, + mmg_3dof_model!, + mmg_3dof_zigzag_test, + estimate_mmg_approx_lsm, + estimate_mmg_approx_lsm_time_window_sampling, + create_model_for_mcmc_sample_mmg + +include("mmg_wind.jl") export Mmg3DofBasicParams, Mmg3DofManeuveringParams, Mmg3DofWindForceMomentParams, apparent_wind_speed_and_angle, mmg_3dof_simulate, - mmg_3dof_model!, + mmg_3dof_model_adbanced!, mmg_3dof_zigzag_test, estimate_mmg_approx_lsm, estimate_mmg_approx_lsm_time_window_sampling, diff --git a/src/data.jl b/src/data.jl index 4bcd28b..fac374d 100644 --- a/src/data.jl +++ b/src/data.jl @@ -1,4 +1,4 @@ -@with_kw struct ShipData{Tt,Tu,Tv,Tr,Tx,Ty,Tψ,Tδ,Tn_p,TU_w,Tψ_w} +@with_kw struct ShipData{Tt,Tu,Tv,Tr,Tx,Ty,Tψ,Tδ,Tn_p} time::Tt u::Tu v::Tv @@ -8,8 +8,20 @@ ψ::Tψ δ::Tδ n_p::Tn_p - U_W::TU_w - ψ_W::Tψ_w +end + +@with_kw struct ShipDataAdvanced{Tt,Tu,Tv,Tr,Tx,Ty,Tψ,Tδ,Tn_p,TU_W,Tψ_W} + time::Tt + u::Tu + v::Tv + r::Tr + x::Tx + y::Ty + ψ::Tψ + δ::Tδ + n_p::Tn_p + U_W::TU_W + ψ_W::Tψ_W end function get_KVLCC2_L7_basic_params(ρ=1025.0) diff --git a/src/mmg.jl b/src/mmg.jl index 2781f84..0404a53 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -1,22 +1,3 @@ -""" - apparent_wind_speed_and_angle(U_W, ψ_W, u, v, ψ) - - ψ_W is the wind coming from the north, with 0 degrees measured clockwise from the direction of the bow of the ship. - ψ_A is the wind coming from the direction of the bow of the ship, with 0 degrees measured clockwise. -""" -function apparent_wind_speed_and_angle(U_W, ψ_W, u, v, Ψ) - - u_A = -U_W * cos(ψ_W) - u * cos(Ψ) + v * sin(Ψ) - v_A = -U_W * sin(ψ_W) - u * sin(Ψ) - v * cos(Ψ) - - U_A = sqrt(u_A^2 + v_A^2) - - ψ_A = π + atan(v_A, u_A) - ψ_W - ψ_A = (ψ_A < 0 ? ψ_A + 2π : ψ_A) % (2π) - - return U_A, ψ_A -end - """ mmg_3dof_model!(dX, X, p, t) @@ -25,7 +6,7 @@ MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`. # Arguments - `dX`: [du, dv, dr, dx, dy, dΨ, dδ, dn_p] - `X`: the initial state values. [`u`, `v`, `r`, `x`, `y`, `Ψ`, `δ`, `n_p`]. -- `p`: ρ and the basic & maneuvering parameters and δ & n_p and three-directional external forces & moment spline info. +- `p`: ρ and the basic & maneuvering parameters and δ & n_p spline info. - ρ - L_pp - B @@ -72,19 +53,12 @@ MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`. - N_vvr_dash - N_vrr_dash - N_rrr_dash - - A_F - - A_L - - spl_C_X - - spl_C_Y - - spl_C_N - spl_δ - spl_n_p - - u_wind_list - - ψ_wind_list - `t`: the time. """ function mmg_3dof_model!(dX, X, p, t) - u, v, r, x, y, ψ, δ, n_p, u_wind, ψ_wind = X + u, v, r, x, y, Ψ, δ, n_p = X ρ, L_pp, B, @@ -131,15 +105,8 @@ function mmg_3dof_model!(dX, X, p, t) N_vvr_dash, N_vrr_dash, N_rrr_dash, - A_F, - A_L, - spl_C_X, - spl_C_Y, - spl_C_N, spl_δ, - spl_n_p, - spl_U_W, - spl_ψ_W = p + spl_n_p = p U = sqrt(u^2 + (v - r * x_G)^2) @@ -246,30 +213,20 @@ function mmg_3dof_model!(dX, X, p, t) N_rrr_dash * (r_dash^3) ) ) - - U_A, ψ_A = apparent_wind_speed_and_angle(spl_U_W(t), spl_ψ_W(t), u, v, Ψ) - ρ_air = 1.225 - - X_wind = ρ_air * A_F * spl_C_X(ψ_A) / 2 * U_A^2 - Y_wind = ρ_air * A_L * spl_C_Y(ψ_A) / 2 * U_A^2 - N_wind = ρ_air * A_L * L_pp * spl_C_N(ψ_A) / 2 * U_A^2 - N_R = -(x_R + a_H * x_H) * F_N * cos(δ) - dX[1] = du = ((X_H + X_R + X_P + X_wind) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) + dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) dX[2] = dv = ( - (x_G^2) * (m^2) * u * r - (N_H + N_R + N_wind) * x_G * m + - ((Y_H + Y_R + Y_wind) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) + (x_G^2) * (m^2) * u * r - (N_H + N_R) * x_G * m + + ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2)) - dX[3] = dr = (N_H + N_R + N_wind - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m) + dX[3] = dr = (N_H + N_R - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m) dX[4] = dx = u * cos(Ψ) - v * sin(Ψ) dX[5] = dy = u * sin(Ψ) + v * cos(Ψ) dX[6] = dΨ = r dX[7] = dδ = derivative(spl_δ, t) dX[8] = dn_p = derivative(spl_n_p, t) - dX[9] = du_wind = derivative(spl_u_wind, t) - dX[10] = dψ_wind = derivative(spl_ψ_wind, t) end """ @@ -379,47 +336,23 @@ Maneuvering parameters of target ship for MMG 3DOF simulation. end """ -Wind force and moment parameters of target ship for MMG 3DOF simulation. - -# Arguments -- `A_F::T` -- `A_L::T` -- `spl_C_X::T` -- `spl_C_Y::T` -- `spl_C_N::T` -""" -@with_kw mutable struct Mmg3DofWindForceMomentParams{T} - A_F::T - A_L::T - spl_C_X::Dierckx.Spline1D - spl_C_Y::Dierckx.Spline1D - spl_C_N::Dierckx.Spline1D -end - - - - -""" - mmg_3dof_simulate(time_list, n_p_list, δ_list, U_W_list, ψ_W_list basic_params, maneuvering_params,wind_force_and_moment_params, [ u0, v0, r0, x0, y0, ψ0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, ψ, δ, n_p + mmg_3dof_simulate(time_list, n_p_list, δ_list, basic_params, maneuvering_params, [, u0, v0, r0, x0, y0, Ψ0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, Ψ, δ, n_p -Returns the MMG 3DOF simulation results including the lists of time, u, v, r, x, y, ψ, δ, n_p. +Returns the MMG 3DOF simulation results including the lists of time, u, v, r, x, y, Ψ, δ, n_p. This function has the same logic of `ShipMMG.simulate()`. # Arguments - `basic_params::Mmg3DofBasicParams`: the basic parameters of target ship. - `maneuvering_params::Mmg3DofManeuveringParams`: the maneuvering parameters of target ship. -- `wind_force_and_moment_params::Mmg3DofWindForceMomentParams,` : the wind force and moment parameters above the taeget ship. - `time_list`: the list of simulatino time. - `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. -- `U_W_list` :the list of wind velocity [m/s]. -- `ψ_W_list` :the list of wind direction [rad]. - `u0=0.0`: the initial x (surge) velocity. - `v0=0.0`: the initial y (sway) velocity. - `r0=0.0`: the initial rate of turn [rad/s]. - `x0=0.0`: the initial x (surge) position. - `y0=0.0`: the initial y (sway) position. -- `ψ0=0.0`: the initial ψ (yaw) azimuth [rad]. +- `Ψ0=0.0`: the initial Ψ (yaw) azimuth [rad]. - `ρ=1025.0`: the seawater density [kg/m^3]. - `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve() - `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() @@ -437,16 +370,12 @@ julia> sampling = duration * 10; julia> time_list = range(0.00, stop = duration, length = sampling); julia> δ_rad_list = max_δ_rad .* ones(Float64, sampling); julia> n_p_list = n_const .* ones(Float64, sampling); -julia> U_W_list = 5 .* ones(Float64, sampling); -julia> ψ_W_list = 0 .* ones(Float64, sampling); julia> mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, time_list, δ_rad_list, n_p_list, - U_W_list, - ψ_W_list, u0 = 2.29 * 0.512, v0 = 0.0, r0 = 0.0, @@ -456,22 +385,19 @@ julia> mmg_results = mmg_3dof_simulate( function mmg_3dof_simulate( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, - wind_force_and_moment_params::Mmg3DofWindForceMomentParams, time_list, δ_list, - n_p_list, - U_W_list, - ψ_W_list; + n_p_list; u0=0.0, v0=0.0, r0=0.0, x0=0.0, y0=0.0, - ψ0=0.0, + Ψ0=0.0, ρ=1025.0, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8, + abstol=1e-8 ) @unpack L_pp, B, @@ -519,9 +445,6 @@ function mmg_3dof_simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash = maneuvering_params - - @unpack A_F, A_L, spl_C_X, spl_C_Y, spl_C_N = wind_force_and_moment_params - simulate( L_pp, B, @@ -568,22 +491,15 @@ function mmg_3dof_simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash, - A_F, - A_L, - spl_C_X, - spl_C_Y, - spl_C_N, time_list, δ_list, n_p_list, - U_W_list, - ψ_W_list; u0=u0, v0=v0, r0=r0, x0=x0, y0=y0, - ψ0=ψ0, + Ψ0=Ψ0, ρ=ρ, algorithm=algorithm, reltol=reltol, @@ -592,9 +508,9 @@ function mmg_3dof_simulate( end """ - simulate(time_list, n_p_list, δ_list, U_W_list, ψ_W_list, L_pp, B, d, x_G, D_p, m, I_zG, A_R, η, m_x, m_y, J_z, f_α, ϵ, t_R, x_R, a_H, x_H, γ_R_minus, γ_R_plus, l_R, κ, t_P, w_P0, x_P, A_OD, A_F, A_L, H_BR, H_C, C, k_0, k_1, k_2, R_0_dash, X_vv_dash, X_vr_dash, X_rr_dash, X_vvvv_dash, Y_v_dash, Y_r_dash, Y_vvv_dash, Y_vvr_dash, Y_vrr_dash, Y_rrr_dash, N_v_dash, N_r_dash, N_vvv_dash, N_vvr_dash, N_vrr_dash, N_rrr_dash, A_F, A_L, spl_C_X, spl_C_Y, spl_C_N, [, u0, v0, r0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, ψ, δ, n_p + simulate(time_list, n_p_list, δ_list, L_pp, B, d, x_G, D_p, m, I_zG, A_R, η, m_x, m_y, J_z, f_α, ϵ, t_R, x_R, a_H, x_H, γ_R_minus, γ_R_plus, l_R, κ, t_P, w_P0, x_P, k_0, k_1, k_2, R_0_dash, X_vv_dash, X_vr_dash, X_rr_dash, X_vvvv_dash, Y_v_dash, Y_r_dash, Y_vvv_dash, Y_vvr_dash, Y_vrr_dash, Y_rrr_dash, N_v_dash, N_r_dash, N_vvv_dash, N_vvr_dash, N_vrr_dash, N_rrr_dash, [, u0, v0, r0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, Ψ, δ, n_p -Returns the MMG 3DOF simulation results including the lists of time, u, v, r, x, y, ψ, δ, n_p. +Returns the MMG 3DOF simulation results including the lists of time, u, v, r, x, y, Ψ, δ, n_p. This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. # Arguments @@ -643,22 +559,15 @@ This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. - `N_vvr_dash` - `N_vrr_dash` - `N_rrr_dash` -- `A_F` -- `A_L` -- `spl_C_X` -- `spl_C_Y` -- `spl_C_N` - `time_list`: the list of simulatino time. - `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. -- `U_W_list` :the list of wind velocity [m/s]. -- `ψ_W_list` :the list of wind direction [rad]. - `u0=0.0`: the initial x (surge) velocity. - `v0=0.0`: the initial y (sway) velocity. - `r0=0.0`: the initial rate of turn [rad/s]. - `x0=0.0`: the initial x (surge) position. - `y0=0.0`: the initial y (sway) position. -- `ψ0=0.0`: the initial ψ (yaw) azimuth [rad]. +- `Ψ0=0.0`: the initial Ψ (yaw) azimuth [rad]. - `ρ=1025.0`: the seawater density [kg/m^3]. - `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve() - `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() @@ -710,22 +619,15 @@ function simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash, - A_F, - A_L, - spl_C_X, - spl_C_Y, - spl_C_N, time_list, δ_list, - n_p_list, - U_W_list, - ψ_W_list; + n_p_list; u0=0.0, v0=0.0, r0=0.0, x0=0.0, y0=0.0, - ψ0=0.0, + Ψ0=0.0, ρ=1025.0, algorithm=Tsit5(), reltol=1e-8, @@ -733,8 +635,6 @@ function simulate( ) spl_δ = Spline1D(time_list, δ_list) spl_n_p = Spline1D(time_list, n_p_list) - spl_U_W = Spline1D(time_list, U_W_list) - spl_ψ_W = Spline1D(time_list, ψ_W_list) X0 = [u0; v0; r0; x0; y0; Ψ0; δ_list[1]; n_p_list[1]] p = ( @@ -784,15 +684,8 @@ function simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash, - A_F, - A_L, - spl_C_X, - spl_C_Y, - spl_C_N, spl_δ, spl_n_p, - spl_U_W, - spl_ψ_W, ) prob = ODEProblem(mmg_3dof_model!, X0, (time_list[1], time_list[end]), p) sol = solve(prob, algorithm, reltol=reltol, abstol=abstol) @@ -803,37 +696,30 @@ function simulate( r = results[3, :] x = results[4, :] y = results[5, :] - ψ = results[6, :] + Ψ = results[6, :] δ = results[7, :] n_p = results[8, :] - u_wind = results[9, :] - ψ_wind = results[10, :] - u, v, r, x, y, ψ, δ, n_p, u_wind, ψ_wind + u, v, r, x, y, Ψ, δ, n_p end """ - mmg_3dof_zigzag_test(basic_params, maneuvering_params,wind_force_and_moment_params time_list, n_p_list, u_wind_list, Ψ_wind_list, target_δ_rad, target_Ψ_rad_deviation, [, u0, v0, r0, x0, y0, Ψ0, δ0, δ_rad_rate, algorithm, reltol, abstol]) -> u, v, r, x, y, ψ, δ + mmg_3dof_zigzag_test(basic_params, maneuvering_params, time_list, n_p_list, target_δ_rad, target_Ψ_rad_deviation, [, u0, v0, r0, x0, y0, Ψ0, δ0, δ_rad_rate, algorithm, reltol, abstol]) -> u, v, r, x, y, Ψ, δ Returns the MMG 3DOF zigzag simulation results. # Arguments - `basic_params::Mmg3DofBasicParams`: the basic parameters of target ship. - `maneuvering_params::Mmg3DofManeuveringParams`: the maneuvering parameters of target ship. -- `wind_force_and_moment_params::Mmg3DofWindForceMomentParams,` : the Structur parameters above the taeget ship's draft - `time_list`: the list of simulatino time. -- `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. - `target_δ_rad`: target rudder angle of zigzag test. - `target_Ψ_rad_deviation`: target azimuth deviation of zigzag test. -- `X_EX_list`: the list of external force in x (surge). -- `Y_EX_list`: the list of external force in y (sway). -- `N_EX_list`: the list of external moment in yaw. - `u0=0.0`: the initial x (surge) velocity. - `v0=0.0`: the initial y (sway) velocity. - `r0=0.0`: the initial rate of turn [rad/s]. - `x0=0.0`: the initial x (surge) position. - `y0=0.0`: the initial y (sway) position. -- `ψ0=0.0`: the initial ψ (yaw) azimuth [rad]. +- `Ψ0=0.0`: the initial Ψ (yaw) azimuth [rad]. - `δ0=0.0`: the initial rudder angle. - `δ_rad_rate=10.0*π/180`: the change rate of rudder angle [rad/s]. - `ρ=1025.0`: the seawater density [kg/m^3]. @@ -848,53 +734,44 @@ KVLCC2_L7 zigzag test. julia> ρ = 1025.0; julia> basic_params, maneuvering_params = get_KVLCC2_L7_params(); julia> target_δ_rad = 20.0 * π / 180.0 -julia> target_ψ_rad_deviation = 20.0 * π / 180.0 +julia> target_Ψ_rad_deviation = 20.0 * π / 180.0 julia> start_time_second = 0.00 julia> time_second_interval = 0.01 julia> end_time_second = 80.00 julia> time_list = start_time_second:time_second_interval:end_time_second julia> n_const = 17.95 # [rps] julia> n_p_list = n_const * ones(Float64, length(time_list)) -julia> X_EX_list = zeros(Float64, length(time_list)) -julia> Y_EX_list = zeros(Float64, length(time_list)) -julia> N_EX_list = zeros(Float64, length(time_list)) julia> δ_list, u_list, v_list, r_list, Ψ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, - wind_force_and_moment_params, - time_list, + time_list n_p_list, - u_wind_list, - ψ_wind_list, target_δ_rad, - target_ψ_rad_deviation, + target_Ψ_rad_deviation, ); ``` """ function mmg_3dof_zigzag_test( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, - wind_force_and_moment_params::Mmg3DofWindForceMomentParams, time_list, n_p_list, target_δ_rad, - target_Ψ_rad_deviation, - U_W_list, - ψ_W_list; + target_Ψ_rad_deviation; u0=0.0, v0=0.0, r0=0.0, x0=0.0, y0=0.0, - ψ0=0.0, + Ψ0=0.0, δ0=0.0, δ_rad_rate=10.0 * π / 180, ρ=1025.0, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8, + abstol=1e-8 ) - target_ψ_rad_deviation = abs(target_ψ_rad_deviation) + target_Ψ_rad_deviation = abs(target_Ψ_rad_deviation) final_δ_list = zeros(length(time_list)) final_u_list = zeros(length(time_list)) @@ -902,11 +779,11 @@ function mmg_3dof_zigzag_test( final_r_list = zeros(length(time_list)) final_x_list = zeros(length(time_list)) final_y_list = zeros(length(time_list)) - final_ψ_list = zeros(length(time_list)) + final_Ψ_list = zeros(length(time_list)) next_stage_index = 1 target_δ_rad = -target_δ_rad # for changing in while loop - ψ = ψ0 + Ψ = Ψ0 while next_stage_index < length(time_list) target_δ_rad = -target_δ_rad start_index = next_stage_index @@ -929,7 +806,7 @@ function mmg_3dof_zigzag_test( y0 = final_y_list[start_index-1] end - for i in (start_index+1):length(time_list) + for i = (start_index+1):length(time_list) Δt = time_list[i] - time_list[i-1] if target_δ_rad > 0 δ = δ_list[i-start_index] + δ_rad_rate * Δt @@ -946,21 +823,18 @@ function mmg_3dof_zigzag_test( end end - u, v, r, x, y, ψ_list, δ, n_p = mmg_3dof_simulate( + u, v, r, x, y, Ψ_list, δ, n_p = mmg_3dof_simulate( basic_params, maneuvering_params, - wind_force_and_moment_params, time_list[start_index:end], δ_list, n_p_list[start_index:end], - U_W_list[start_index:end], - ψ_W_list[start_index:end]; u0=u0, v0=v0, r0=r0, x0=x0, y0=y0, - ψ0=ψ, + Ψ0=Ψ, ρ=ρ, algorithm=algorithm, reltol=reltol, @@ -968,13 +842,13 @@ function mmg_3dof_zigzag_test( ) # get finish index - target_ψ_rad = ψ0 + target_ψ_rad_deviation + target_Ψ_rad = Ψ0 + target_Ψ_rad_deviation if target_δ_rad < 0 - target_ψ_rad = ψ0 - target_ψ_rad_deviation + target_Ψ_rad = Ψ0 - target_Ψ_rad_deviation end - over_index = findfirst(e -> e > target_ψ_rad, ψ_list) + over_index = findfirst(e -> e > target_Ψ_rad, Ψ_list) if target_δ_rad < 0 - over_index = findfirst(e -> e < target_ψ_rad, ψ_list) + over_index = findfirst(e -> e < target_Ψ_rad, Ψ_list) end next_stage_index = length(time_list) if isnothing(over_index) @@ -984,9 +858,9 @@ function mmg_3dof_zigzag_test( final_r_list[start_index:next_stage_index] = r final_x_list[start_index:next_stage_index] = x final_y_list[start_index:next_stage_index] = y - final_ψ_list[start_index:next_stage_index] = ψ_list + final_Ψ_list[start_index:next_stage_index] = Ψ_list else - ψ = ψ_list[over_index] + Ψ = Ψ_list[over_index] next_stage_index = over_index + start_index - 1 final_δ_list[start_index:next_stage_index-1] = δ_list[begin:over_index-1] final_u_list[start_index:next_stage_index-1] = u[begin:over_index-1] @@ -994,7 +868,7 @@ function mmg_3dof_zigzag_test( final_r_list[start_index:next_stage_index-1] = r[begin:over_index-1] final_x_list[start_index:next_stage_index-1] = x[begin:over_index-1] final_y_list[start_index:next_stage_index-1] = y[begin:over_index-1] - final_ψ_list[start_index:next_stage_index-1] = ψ_list[begin:over_index-1] + final_Ψ_list[start_index:next_stage_index-1] = Ψ_list[begin:over_index-1] end end final_u_list, @@ -1002,14 +876,13 @@ function mmg_3dof_zigzag_test( final_r_list, final_x_list, final_y_list, - final_ψ_list, + final_Ψ_list, final_δ_list end function create_model_for_mcmc_sample_mmg( data::ShipData, basic_params::Mmg3DofBasicParams, - wind_force_and_moment_params::Mmg3DofWindForceMomentParams, k_0, k_1, k_2; @@ -1036,17 +909,14 @@ function create_model_for_mcmc_sample_mmg( N_rrr_dash_prior_dist=Uniform(-0.060, 0.000), solver=Tsit5(), abstol=1e-6, - reltol=1e-3, + reltol=1e-3 ) time_obs = data.time u_obs = data.u v_obs = data.v r_obs = data.r - ψ_obs = data.ψ δ_obs = data.δ n_p_obs = data.n_p - U_W_obs = data.U_W - ψ_W_obs = data.ψ_W @unpack L_pp, B, @@ -1074,16 +944,11 @@ function create_model_for_mcmc_sample_mmg( w_P0, x_P = basic_params - @unpack A_F, A_L, spl_C_X, spl_C_Y, spl_C_N = wind_force_and_moment_params - # create sytem model spl_δ = Spline1D(time_obs, δ_obs) spl_n_p = Spline1D(time_obs, n_p_obs) - spl_U_W = Spline1D(time_obs, U_W_obs) - spl_ψ_W = Spline1D(time_obs, ψ_W_obs) - function MMG!(dX, X, p, t) - u, v, r, Ψ, δ, n_p = X + u, v, r, δ, n_p = X R_0_dash, X_vv_dash, X_vr_dash, @@ -1153,10 +1018,7 @@ function create_model_for_mcmc_sample_mmg( u * (1.0 - w_P) * ϵ * - sqrt( - η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + - (1 - η), - ) + sqrt(η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + (1 - η)) end U_R = sqrt(u_R^2 + v_R^2) @@ -1211,26 +1073,16 @@ function create_model_for_mcmc_sample_mmg( ) ) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) - - U_A, ψ_A = apparent_wind_speed_and_angle(spl_U_W(t), spl_ψ_W(t), u, v, Ψ) - ρ_air = 1.225 - - ψ_A = ForwardDiff.value(ψ_A) - X_wind = ρ_air * A_F * spl_C_X(ψ_A) / 2 * U_A^2 - Y_wind = ρ_air * A_L * spl_C_Y(ψ_A) / 2 * U_A^2 - N_wind = ρ_air * A_L * L_pp * spl_C_N(ψ_A) / 2 * U_A^2 - - dX[1] = du = ((X_H + X_R + X_P + X_wind) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) + dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) dX[2] = dv = ( - (x_G^2) * (m^2) * u * r - (N_H + N_R + N_wind) * x_G * m + - ((Y_H + Y_R + Y_wind) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) + (x_G^2) * (m^2) * u * r - (N_H + N_R) * x_G * m + + ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2)) - dX[3] = dr = (N_H + N_R + N_wind - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m) - dX[4] = dΨ = r - dX[5] = dδ = derivative(spl_δ, t) - dX[6] = dn_p = derivative(spl_n_p, t) + dX[3] = dr = (N_H + N_R - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m) + dX[4] = dδ = derivative(spl_δ, t) + dX[5] = dn_p = derivative(spl_n_p, t) end R_0_dash_start = 0.022 @@ -1274,11 +1126,11 @@ function create_model_for_mcmc_sample_mmg( u0 = 2.29 * 0.512 v0 = 0.0 r0 = 0.0 - X0 = [u_obs[1]; v_obs[1]; r_obs[1]; ψ_obs[1]; δ_obs[1]; n_p_obs[1]] + X0 = [u_obs[1]; v_obs[1]; r_obs[1]; δ_obs[1]; n_p_obs[1]] prob1 = ODEProblem(MMG!, X0, (time_obs[1], time_obs[end]), p) # create probabilistic model - Turing.@model function fitMMG(time_obs, obs, prob1) + @model function fitMMG(time_obs, obs, prob1) σ_u ~ σ_u_prior_dist σ_v ~ σ_v_prior_dist σ_r ~ σ_r_prior_dist diff --git a/src/mmg_wind.jl b/src/mmg_wind.jl new file mode 100644 index 0000000..1117495 --- /dev/null +++ b/src/mmg_wind.jl @@ -0,0 +1,1222 @@ +""" + apparent_wind_speed_and_angle(U_W, Ψ_W, u, v, Ψ) + + Ψ_W is the wind coming from the north, with 0 degrees measured clockwise from the direction of the bow of the ship. + Ψ_A is the wind coming from the direction of the bow of the ship, with 0 degrees measured clockwise. +""" +function apparent_wind_speed_and_angle(U_W, Ψ_W, u, v, Ψ) + + u_A = -U_W * cos(Ψ_W) - u * cos(Ψ) + v * sin(Ψ) + v_A = -U_W * sin(Ψ_W) - u * sin(Ψ) - v * cos(Ψ) + + U_A = sqrt(u_A^2 + v_A^2) + + Ψ_A = π + atan(v_A, u_A) - Ψ_W + Ψ_A = (Ψ_A < 0 ? Ψ_A + 2π : Ψ_A) % (2π) + + return U_A, Ψ_A +end + +""" + mmg_3dof_model_adbanced!(dX, X, p, t) + +MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`. + +# Arguments +- `dX`: [du, dv, dr, dx, dy, dΨ, dδ, dn_p] +- `X`: the initial state values. [`u`, `v`, `r`, `x`, `y`, `Ψ`, `δ`, `n_p`]. +- `p`: ρ and the basic & maneuvering parameters and δ & n_p and three-directional external forces & moment spline info. + - ρ + - L_pp + - B + - d + - x_G + - D_p + - m + - I_zG + - A_R + - η + - m_x + - m_y + - J_z + - f_α + - ϵ + - t_R + - x_R + - a_H + - x_H + - γ_R_minus + - γ_R_plus + - l_R + - κ + - t_P + - w_P0 + - x_P + - k_0 + - k_1 + - k_2 + - R_0_dash + - X_vv_dash + - X_vr_dash + - X_rr_dash + - X_vvvv_dash + - Y_v_dash + - Y_r_dash + - Y_vvv_dash + - Y_vvr_dash + - Y_vrr_dash + - Y_rrr_dash + - N_v_dash + - N_r_dash + - N_vvv_dash + - N_vvr_dash + - N_vrr_dash + - N_rrr_dash + - A_F + - A_L + - spl_C_X + - spl_C_Y + - spl_C_N + - spl_δ + - spl_n_p + - U_W_list + - Ψ_W_list +- `t`: the time. +""" +function mmg_3dof_model_adbanced!(dX, X, p, t) + u, v, r, x, y, Ψ, δ, n_p = X + ρ, + L_pp, + B, + d, + x_G, + D_p, + m, + I_zG, + A_R, + η, + m_x, + m_y, + J_z, + f_α, + ϵ, + t_R, + x_R, + a_H, + x_H, + γ_R_minus, + γ_R_plus, + l_R, + κ, + t_P, + w_P0, + x_P, + k_0, + k_1, + k_2, + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash, + A_F, + A_L, + spl_C_X, + spl_C_Y, + spl_C_N, + spl_δ, + spl_n_p, + spl_U_W, + spl_Ψ_W = p + + U = sqrt(u^2 + (v - r * x_G)^2) + + β = 0.0 + if U == 0.0 + β = 0.0 + else + β = asin(-(v - r * x_G) / U) + end + + v_dash = 0.0 + if U == 0.0 + v_dash = 0.0 + else + v_dash = v / U + end + + r_dash = 0.0 + if U == 0.0 + r_dash = 0.0 + else + r_dash = r * L_pp / U + end + + w_P = w_P0 * exp(-4.0 * (β - x_P * r_dash)^2) + + J = 0.0 + if n_p == 0.0 + J = 0.0 + else + J = (1 - w_P) * u / (n_p * D_p) + end + K_T = k_0 + k_1 * J + k_2 * J^2 + β_R = β - l_R * r_dash + γ_R = γ_R_minus + + if β_R < 0.0 + γ_R = γ_R_minus + else + γ_R = γ_R_plus + end + + v_R = U * γ_R * β_R + + u_R = 0.0 + if J == 0.0 + u_R = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p^2 * D_p^4 / pi)^2) + else + u_R = + u * + (1.0 - w_P) * + ϵ * + sqrt(η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + (1 - η)) + end + + U_R = sqrt(u_R^2 + v_R^2) + α_R = δ - atan(v_R, u_R) + F_N = 0.5 * A_R * ρ * f_α * (U_R^2) * sin(α_R) + + X_H = ( + 0.5 * + ρ * + L_pp * + d * + (U^2) * + ( + -R_0_dash + + X_vv_dash * (v_dash^2) + + X_vr_dash * v_dash * r_dash + + X_rr_dash * (r_dash^2) + + X_vvvv_dash * (v_dash^4) + ) + ) + X_R = -(1.0 - t_R) * F_N * sin(δ) + X_P = (1.0 - t_P) * ρ * K_T * n_p^2 * D_p^4 + Y_H = ( + 0.5 * + ρ * + L_pp * + d * + (U^2) * + ( + Y_v_dash * v_dash + + Y_r_dash * r_dash + + Y_vvv_dash * (v_dash^3) + + Y_vvr_dash * (v_dash^2) * r_dash + + Y_vrr_dash * v_dash * (r_dash^2) + + Y_rrr_dash * (r_dash^3) + ) + ) + Y_R = -(1 + a_H) * F_N * cos(δ) + N_H = ( + 0.5 * + ρ * + (L_pp^2) * + d * + (U^2) * + ( + N_v_dash * v_dash + + N_r_dash * r_dash + + N_vvv_dash * (v_dash^3) + + N_vvr_dash * (v_dash^2) * r_dash + + N_vrr_dash * v_dash * (r_dash^2) + + N_rrr_dash * (r_dash^3) + ) + ) + + U_A, Ψ_A = apparent_wind_speed_and_angle(spl_U_W(t), spl_Ψ_W(t), u, v, Ψ) + ρ_air = 1.225 + + X_wind = ρ_air * A_F * spl_C_X(Ψ_A) / 2 * U_A^2 + Y_wind = ρ_air * A_L * spl_C_Y(Ψ_A) / 2 * U_A^2 + N_wind = ρ_air * A_L * L_pp * spl_C_N(Ψ_A) / 2 * U_A^2 + + N_R = -(x_R + a_H * x_H) * F_N * cos(δ) + dX[1] = du = ((X_H + X_R + X_P + X_wind) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) + dX[2] = + dv = + ( + (x_G^2) * (m^2) * u * r - (N_H + N_R + N_wind) * x_G * m + + ((Y_H + Y_R + Y_wind) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) + ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2)) + dX[3] = dr = (N_H + N_R + N_wind - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m) + dX[4] = dx = u * cos(Ψ) - v * sin(Ψ) + dX[5] = dy = u * sin(Ψ) + v * cos(Ψ) + dX[6] = dΨ = r + dX[7] = dδ = derivative(spl_δ, t) + dX[8] = dn_p = derivative(spl_n_p, t) +end + + +""" +Wind force and moment parameters of target ship for MMG 3DOF simulation. + +# Arguments +- `A_F::T` +- `A_L::T` +- `spl_C_X::T` +- `spl_C_Y::T` +- `spl_C_N::T` +""" +@with_kw mutable struct Mmg3DofWindForceMomentParams{T} + A_F::T + A_L::T + spl_C_X::Dierckx.Spline1D + spl_C_Y::Dierckx.Spline1D + spl_C_N::Dierckx.Spline1D +end + + +""" + mmg_3dof_simulate(time_list, n_p_list, δ_list, U_W_list, Ψ_W_list basic_params, maneuvering_params,wind_force_and_moment_params, [ u0, v0, r0, x0, y0, Ψ0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, Ψ, δ, n_p + +Returns the MMG 3DOF with wind simulation results including the lists of time, u, v, r, x, y, Ψ, δ, n_p. +This function has the same logic of `ShipMMG.simulate()`. + +# Arguments +- `basic_params::Mmg3DofBasicParams`: the basic parameters of target ship. +- `maneuvering_params::Mmg3DofManeuveringParams`: the maneuvering parameters of target ship. +- `wind_force_and_moment_params::Mmg3DofWindForceMomentParams,` : the wind force and moment parameters above the taeget ship. +- `time_list`: the list of simulatino time. +- `δ_list`: the list of rudder angle [rad]. +- `n_p_list`: the list of propeller rps. +- `U_W_list` :the list of wind velocity [m/s]. +- `Ψ_W_list` :the list of wind direction [rad]. +- `u0=0.0`: the initial x (surge) velocity. +- `v0=0.0`: the initial y (sway) velocity. +- `r0=0.0`: the initial rate of turn [rad/s]. +- `x0=0.0`: the initial x (surge) position. +- `y0=0.0`: the initial y (sway) position. +- `Ψ0=0.0`: the initial Ψ (yaw) azimuth [rad]. +- `ρ=1025.0`: the seawater density [kg/m^3]. +- `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve() +- `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() +- `abstol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() + +# Examples +KVLCC2_L7 turning test. + +```julia-rep1 +julia> basic_params, maneuvering_params = get_KVLCC2_L7_params(); +julia> duration = 200; # [s] +julia> max_δ_rad = 35 * pi / 180.0; # [rad] +julia> n_const = 17.95; # [rps] +julia> sampling = duration * 10; +julia> time_list = range(0.00, stop = duration, length = sampling); +julia> δ_rad_list = max_δ_rad .* ones(Float64, sampling); +julia> n_p_list = n_const .* ones(Float64, sampling); +julia> U_W_list = 5 .* ones(Float64, sampling); +julia> Ψ_W_list = 0 .* ones(Float64, sampling); +julia> mmg_results = mmg_3dof_simulate( + basic_params, + maneuvering_params, + time_list, + δ_rad_list, + n_p_list, + U_W_list, + Ψ_W_list, + u0 = 2.29 * 0.512, + v0 = 0.0, + r0 = 0.0, +); +``` +""" +function mmg_3dof_simulate( + basic_params::Mmg3DofBasicParams, + maneuvering_params::Mmg3DofManeuveringParams, + wind_force_and_moment_params::Mmg3DofWindForceMomentParams, + time_list, + δ_list, + n_p_list, + U_W_list, + Ψ_W_list; + u0=0.0, + v0=0.0, + r0=0.0, + x0=0.0, + y0=0.0, + Ψ0=0.0, + ρ=1025.0, + algorithm=Tsit5(), + reltol=1e-8, + abstol=1e-8, +) + @unpack L_pp, + B, + d, + x_G, + D_p, + m, + I_zG, + A_R, + η, + m_x, + m_y, + J_z, + f_α, + ϵ, + t_R, + x_R, + a_H, + x_H, + γ_R_minus, + γ_R_plus, + l_R, + κ, + t_P, + w_P0, + x_P = basic_params + + @unpack k_0, + k_1, + k_2, + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash = maneuvering_params + + @unpack A_F, A_L, spl_C_X, spl_C_Y, spl_C_N = wind_force_and_moment_params + + simulate( + L_pp, + B, + d, + x_G, + D_p, + m, + I_zG, + A_R, + η, + m_x, + m_y, + J_z, + f_α, + ϵ, + t_R, + x_R, + a_H, + x_H, + γ_R_minus, + γ_R_plus, + l_R, + κ, + t_P, + w_P0, + x_P, + k_0, + k_1, + k_2, + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash, + A_F, + A_L, + spl_C_X, + spl_C_Y, + spl_C_N, + time_list, + δ_list, + n_p_list, + U_W_list, + Ψ_W_list; + u0=u0, + v0=v0, + r0=r0, + x0=x0, + y0=y0, + Ψ0=Ψ0, + ρ=ρ, + algorithm=algorithm, + reltol=reltol, + abstol=abstol, + ) +end + +""" + simulate(time_list, n_p_list, δ_list, U_W_list, Ψ_W_list, L_pp, B, d, x_G, D_p, m, I_zG, A_R, η, m_x, m_y, J_z, f_α, ϵ, t_R, x_R, a_H, x_H, γ_R_minus, γ_R_plus, l_R, κ, t_P, w_P0, x_P, A_OD, A_F, A_L, H_BR, H_C, C, k_0, k_1, k_2, R_0_dash, X_vv_dash, X_vr_dash, X_rr_dash, X_vvvv_dash, Y_v_dash, Y_r_dash, Y_vvv_dash, Y_vvr_dash, Y_vrr_dash, Y_rrr_dash, N_v_dash, N_r_dash, N_vvv_dash, N_vvr_dash, N_vrr_dash, N_rrr_dash, A_F, A_L, spl_C_X, spl_C_Y, spl_C_N, [, u0, v0, r0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, Ψ, δ, n_p + +Returns the MMG 3DOF with wind simulation results including the lists of time, u, v, r, x, y, Ψ, δ, n_p. +This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. + +# Arguments +- `L_pp`: L_pp +- `B`: +- `d`: +- `x_G`: +- `D_p`: +- `m`: +- `I_zG`: +- `A_R`: +- `η`: +- `m_x`: +- `m_y`: +- `J_z`: +- `f_α`: +- `ϵ`: +- `t_R`: +- `x_R`: +- `a_H`: +- `x_H`: +- `γ_R_minus`: +- `γ_R_plus`: +- `l_R`: +- `κ`: +- `t_P`: +- `w_P0`: +- `x_P`: +- `k_0` +- `k_1` +- `k_2` +- `R_0_dash` +- `X_vv_dash` +- `X_vr_dash` +- `X_rr_dash` +- `X_vvvv_dash` +- `Y_v_dash` +- `Y_r_dash` +- `Y_vvv_dash` +- `Y_vvr_dash` +- `Y_vrr_dash` +- `Y_rrr_dash` +- `N_v_dash` +- `N_r_dash` +- `N_vvv_dash` +- `N_vvr_dash` +- `N_vrr_dash` +- `N_rrr_dash` +- `A_F` +- `A_L` +- `spl_C_X` +- `spl_C_Y` +- `spl_C_N` +- `time_list`: the list of simulatino time. +- `δ_list`: the list of rudder angle [rad]. +- `n_p_list`: the list of propeller rps. +- `U_W_list` :the list of wind velocity [m/s]. +- `Ψ_W_list` :the list of wind direction [rad]. +- `u0=0.0`: the initial x (surge) velocity. +- `v0=0.0`: the initial y (sway) velocity. +- `r0=0.0`: the initial rate of turn [rad/s]. +- `x0=0.0`: the initial x (surge) position. +- `y0=0.0`: the initial y (sway) position. +- `Ψ0=0.0`: the initial Ψ (yaw) azimuth [rad]. +- `ρ=1025.0`: the seawater density [kg/m^3]. +- `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve() +- `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() +- `abstol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() +""" +function simulate( + L_pp, + B, + d, + x_G, + D_p, + m, + I_zG, + A_R, + η, + m_x, + m_y, + J_z, + f_α, + ϵ, + t_R, + x_R, + a_H, + x_H, + γ_R_minus, + γ_R_plus, + l_R, + κ, + t_P, + w_P0, + x_P, + k_0, + k_1, + k_2, + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash, + A_F, + A_L, + spl_C_X, + spl_C_Y, + spl_C_N, + time_list, + δ_list, + n_p_list, + U_W_list, + Ψ_W_list; + u0=0.0, + v0=0.0, + r0=0.0, + x0=0.0, + y0=0.0, + Ψ0=0.0, + ρ=1025.0, + algorithm=Tsit5(), + reltol=1e-8, + abstol=1e-8, +) + spl_δ = Spline1D(time_list, δ_list) + spl_n_p = Spline1D(time_list, n_p_list) + spl_U_W = Spline1D(time_list, U_W_list) + spl_Ψ_W = Spline1D(time_list, Ψ_W_list) + + X0 = [u0; v0; r0; x0; y0; Ψ0; δ_list[1]; n_p_list[1]] + p = ( + ρ, + L_pp, + B, + d, + x_G, + D_p, + m, + I_zG, + A_R, + η, + m_x, + m_y, + J_z, + f_α, + ϵ, + t_R, + x_R, + a_H, + x_H, + γ_R_minus, + γ_R_plus, + l_R, + κ, + t_P, + w_P0, + x_P, + k_0, + k_1, + k_2, + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash, + A_F, + A_L, + spl_C_X, + spl_C_Y, + spl_C_N, + spl_δ, + spl_n_p, + spl_U_W, + spl_Ψ_W, + ) + prob = ODEProblem(mmg_3dof_model_adbanced!, X0, (time_list[1], time_list[end]), p) + sol = solve(prob, algorithm, reltol=reltol, abstol=abstol) + sol_timelist = sol(time_list) + results = hcat(sol_timelist.u...) + u = results[1, :] + v = results[2, :] + r = results[3, :] + x = results[4, :] + y = results[5, :] + Ψ = results[6, :] + δ = results[7, :] + n_p = results[8, :] + u, v, r, x, y, Ψ, δ, n_p +end + +""" + mmg_3dof_zigzag_test(basic_params, maneuvering_params,wind_force_and_moment_params time_list, n_p_list, U_W_list, Ψ_W_list, target_δ_rad, target_Ψ_rad_deviation, [, u0, v0, r0, x0, y0, Ψ0, δ0, δ_rad_rate, algorithm, reltol, abstol]) -> u, v, r, x, y, Ψ, δ + +Returns the MMG 3DOF zigzag simulation results. + +# Arguments +- `basic_params::Mmg3DofBasicParams`: the basic parameters of target ship. +- `maneuvering_params::Mmg3DofManeuveringParams`: the maneuvering parameters of target ship. +- `wind_force_and_moment_params::Mmg3DofWindForceMomentParams,` : the Structur parameters above the taeget ship's draft +- `time_list`: the list of simulatino time. +- `δ_list`: the list of rudder angle [rad]. +- `n_p_list`: the list of propeller rps. +- `target_δ_rad`: target rudder angle of zigzag test. +- `target_Ψ_rad_deviation`: target azimuth deviation of zigzag test. +- `X_EX_list`: the list of external force in x (surge). +- `Y_EX_list`: the list of external force in y (sway). +- `N_EX_list`: the list of external moment in yaw. +- `u0=0.0`: the initial x (surge) velocity. +- `v0=0.0`: the initial y (sway) velocity. +- `r0=0.0`: the initial rate of turn [rad/s]. +- `x0=0.0`: the initial x (surge) position. +- `y0=0.0`: the initial y (sway) position. +- `Ψ0=0.0`: the initial Ψ (yaw) azimuth [rad]. +- `δ0=0.0`: the initial rudder angle. +- `δ_rad_rate=10.0*π/180`: the change rate of rudder angle [rad/s]. +- `ρ=1025.0`: the seawater density [kg/m^3]. +- `algorithm=Tsit5()`: the parameter of DifferentialEquations.ODEProblem.solve() +- `reltol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() +- `abstol=1e-8`: the parameter of DifferentialEquations.ODEProblem.solve() + +# Examples +KVLCC2_L7 zigzag test. + +```julia-rep1 +julia> ρ = 1025.0; +julia> basic_params, maneuvering_params = get_KVLCC2_L7_params(); +julia> target_δ_rad = 20.0 * π / 180.0 +julia> target_Ψ_rad_deviation = 20.0 * π / 180.0 +julia> start_time_second = 0.00 +julia> time_second_interval = 0.01 +julia> end_time_second = 80.00 +julia> time_list = start_time_second:time_second_interval:end_time_second +julia> n_const = 17.95 # [rps] +julia> n_p_list = n_const * ones(Float64, length(time_list)) +julia> X_EX_list = zeros(Float64, length(time_list)) +julia> Y_EX_list = zeros(Float64, length(time_list)) +julia> N_EX_list = zeros(Float64, length(time_list)) +julia> δ_list, u_list, v_list, r_list, Ψ_list = mmg_3dof_zigzag_test( + basic_params, + maneuvering_params, + wind_force_and_moment_params, + time_list, + n_p_list, + U_W_list, + Ψ_W_list, + target_δ_rad, + target_Ψ_rad_deviation, +); +``` +""" +function mmg_3dof_zigzag_test( + basic_params::Mmg3DofBasicParams, + maneuvering_params::Mmg3DofManeuveringParams, + wind_force_and_moment_params::Mmg3DofWindForceMomentParams, + time_list, + n_p_list, + target_δ_rad, + target_Ψ_rad_deviation, + U_W_list, + Ψ_W_list; + u0=0.0, + v0=0.0, + r0=0.0, + x0=0.0, + y0=0.0, + Ψ0=0.0, + δ0=0.0, + δ_rad_rate=10.0 * π / 180, + ρ=1025.0, + algorithm=Tsit5(), + reltol=1e-8, + abstol=1e-8, +) + target_Ψ_rad_deviation = abs(target_Ψ_rad_deviation) + + final_δ_list = zeros(length(time_list)) + final_u_list = zeros(length(time_list)) + final_v_list = zeros(length(time_list)) + final_r_list = zeros(length(time_list)) + final_x_list = zeros(length(time_list)) + final_y_list = zeros(length(time_list)) + final_Ψ_list = zeros(length(time_list)) + + next_stage_index = 1 + target_δ_rad = -target_δ_rad # for changing in while loop + Ψ = Ψ0 + while next_stage_index < length(time_list) + target_δ_rad = -target_δ_rad + start_index = next_stage_index + + # Make delta list + δ_list = zeros(length(time_list) - start_index + 1) + if start_index == 1 + δ_list[1] = δ0 + u0 = u0 + v0 = v0 + r0 = r0 + x0 = x0 + y0 = y0 + else + δ_list[1] = final_δ_list[start_index-1] + u0 = final_u_list[start_index-1] + v0 = final_v_list[start_index-1] + r0 = final_r_list[start_index-1] + x0 = final_x_list[start_index-1] + y0 = final_y_list[start_index-1] + end + + for i in (start_index+1):length(time_list) + Δt = time_list[i] - time_list[i-1] + if target_δ_rad > 0 + δ = δ_list[i-start_index] + δ_rad_rate * Δt + if δ >= target_δ_rad + δ = target_δ_rad + end + δ_list[i-start_index+1] = δ + elseif target_δ_rad <= 0 + δ = δ_list[i-start_index] - δ_rad_rate * Δt + if δ <= target_δ_rad + δ = target_δ_rad + end + δ_list[i-start_index+1] = δ + end + end + + u, v, r, x, y, Ψ_list, δ, n_p = mmg_3dof_simulate( + basic_params, + maneuvering_params, + wind_force_and_moment_params, + time_list[start_index:end], + δ_list, + n_p_list[start_index:end], + U_W_list[start_index:end], + Ψ_W_list[start_index:end]; + u0=u0, + v0=v0, + r0=r0, + x0=x0, + y0=y0, + Ψ0=Ψ, + ρ=ρ, + algorithm=algorithm, + reltol=reltol, + abstol=abstol, + ) + + # get finish index + target_Ψ_rad = Ψ0 + target_Ψ_rad_deviation + if target_δ_rad < 0 + target_Ψ_rad = Ψ0 - target_Ψ_rad_deviation + end + over_index = findfirst(e -> e > target_Ψ_rad, Ψ_list) + if target_δ_rad < 0 + over_index = findfirst(e -> e < target_Ψ_rad, Ψ_list) + end + next_stage_index = length(time_list) + if isnothing(over_index) + final_δ_list[start_index:next_stage_index] = δ_list + final_u_list[start_index:next_stage_index] = u + final_v_list[start_index:next_stage_index] = v + final_r_list[start_index:next_stage_index] = r + final_x_list[start_index:next_stage_index] = x + final_y_list[start_index:next_stage_index] = y + final_Ψ_list[start_index:next_stage_index] = Ψ_list + else + Ψ = Ψ_list[over_index] + next_stage_index = over_index + start_index - 1 + final_δ_list[start_index:next_stage_index-1] = δ_list[begin:over_index-1] + final_u_list[start_index:next_stage_index-1] = u[begin:over_index-1] + final_v_list[start_index:next_stage_index-1] = v[begin:over_index-1] + final_r_list[start_index:next_stage_index-1] = r[begin:over_index-1] + final_x_list[start_index:next_stage_index-1] = x[begin:over_index-1] + final_y_list[start_index:next_stage_index-1] = y[begin:over_index-1] + final_Ψ_list[start_index:next_stage_index-1] = Ψ_list[begin:over_index-1] + end + end + final_u_list, + final_v_list, + final_r_list, + final_x_list, + final_y_list, + final_Ψ_list, + final_δ_list +end + +function create_model_for_mcmc_sample_mmg( + data::ShipDataAdvanced, + basic_params::Mmg3DofBasicParams, + wind_force_and_moment_params::Mmg3DofWindForceMomentParams, + k_0, + k_1, + k_2; + ρ=1025.0, + σ_u_prior_dist=Uniform(0.00, 0.20), + σ_v_prior_dist=Uniform(0.00, 0.20), + σ_r_prior_dist=Uniform(0.00, 0.20), + R_0_dash_prior_dist=Uniform(0.000, 0.100), + X_vv_dash_prior_dist=Uniform(-0.200, 0.200), + X_vr_dash_prior_dist=Uniform(-0.223, 0.177), + X_rr_dash_prior_dist=Uniform(-0.088, 0.032), + X_vvvv_dash_prior_dist=Uniform(-1.400, 1.400), + Y_v_dash_prior_dist=Uniform(-0.500, 0.000), + Y_r_dash_prior_dist=Uniform(-0.100, 0.200), + Y_vvv_dash_prior_dist=Uniform(-6.000, 2.000), + Y_vvr_dash_prior_dist=Uniform(-2.500, 1.000), + Y_vrr_dash_prior_dist=Uniform(-1.500, 0.000), + Y_rrr_dash_prior_dist=Uniform(-0.120, 0.040), + N_v_dash_prior_dist=Uniform(-0.200, 0.000), + N_r_dash_prior_dist=Uniform(-0.100, 0.000), + N_vvv_dash_prior_dist=Uniform(-0.500, 0.400), + N_vvr_dash_prior_dist=Uniform(-1.000, 0.000), + N_vrr_dash_prior_dist=Uniform(-0.300, 0.300), + N_rrr_dash_prior_dist=Uniform(-0.060, 0.000), + solver=Tsit5(), + abstol=1e-6, + reltol=1e-3, +) + time_obs = data.time + u_obs = data.u + v_obs = data.v + r_obs = data.r + Ψ_obs = data.ψ + δ_obs = data.δ + n_p_obs = data.n_p + U_W_obs = data.U_W + Ψ_W_obs = data.ψ_W + + @unpack L_pp, + B, + d, + x_G, + D_p, + m, + I_zG, + A_R, + η, + m_x, + m_y, + J_z, + f_α, + ϵ, + t_R, + x_R, + a_H, + x_H, + γ_R_minus, + γ_R_plus, + l_R, + κ, + t_P, + w_P0, + x_P = basic_params + + @unpack A_F, A_L, spl_C_X, spl_C_Y, spl_C_N = wind_force_and_moment_params + + # create sytem model + spl_δ = Spline1D(time_obs, δ_obs) + spl_n_p = Spline1D(time_obs, n_p_obs) + spl_U_W = Spline1D(time_obs, U_W_obs) + spl_Ψ_W = Spline1D(time_obs, Ψ_W_obs) + + function MMG!(dX, X, p, t) + u, v, r, Ψ, δ, n_p = X + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash = p + + U = sqrt(u^2 + (v - r * x_G)^2) + + β = 0.0 + if U == 0.0 + β = 0.0 + else + β = asin(-(v - r * x_G) / U) + end + + v_dash = 0.0 + if U == 0.0 + v_dash = 0.0 + else + v_dash = v / U + end + + r_dash = 0.0 + if U == 0.0 + r_dash = 0.0 + else + r_dash = r * L_pp / U + end + + w_P = w_P0 * exp(-4.0 * (β - x_P * r_dash)^2) + + J = 0.0 + if n_p == 0.0 + J = 0.0 + else + J = (1 - w_P) * u / (n_p * D_p) + end + K_T = k_0 + k_1 * J + k_2 * J^2 + β_R = β - l_R * r_dash + γ_R = γ_R_minus + + if β_R < 0.0 + γ_R = γ_R_minus + else + γ_R = γ_R_plus + end + + v_R = U * γ_R * β_R + + u_R = 0.0 + if J == 0.0 + u_R = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p^2 * D_p^4 / pi)^2) + else + u_R = + u * + (1.0 - w_P) * + ϵ * + sqrt( + η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + + (1 - η), + ) + end + + U_R = sqrt(u_R^2 + v_R^2) + α_R = δ - atan(v_R, u_R) + F_N = 0.5 * A_R * ρ * f_α * (U_R^2) * sin(α_R) + + X_H = ( + 0.5 * + ρ * + L_pp * + d * + (U^2) * + ( + -R_0_dash + + X_vv_dash * (v_dash^2) + + X_vr_dash * v_dash * r_dash + + X_rr_dash * (r_dash^2) + + X_vvvv_dash * (v_dash^4) + ) + ) + X_R = -(1.0 - t_R) * F_N * sin(δ) + X_P = (1.0 - t_P) * ρ * K_T * n_p^2 * D_p^4 + Y_H = ( + 0.5 * + ρ * + L_pp * + d * + (U^2) * + ( + Y_v_dash * v_dash + + Y_r_dash * r_dash + + Y_vvv_dash * (v_dash^3) + + Y_vvr_dash * (v_dash^2) * r_dash + + Y_vrr_dash * v_dash * (r_dash^2) + + Y_rrr_dash * (r_dash^3) + ) + ) + Y_R = -(1 + a_H) * F_N * cos(δ) + N_H = ( + 0.5 * + ρ * + (L_pp^2) * + d * + (U^2) * + ( + N_v_dash * v_dash + + N_r_dash * r_dash + + N_vvv_dash * (v_dash^3) + + N_vvr_dash * (v_dash^2) * r_dash + + N_vrr_dash * v_dash * (r_dash^2) + + N_rrr_dash * (r_dash^3) + ) + ) + N_R = -(x_R + a_H * x_H) * F_N * cos(δ) + + U_A, Ψ_A = apparent_wind_speed_and_angle(spl_U_W(t), spl_Ψ_W(t), u, v, Ψ) + ρ_air = 1.225 + + Ψ_A = ForwardDiff.value(Ψ_A) + X_wind = ρ_air * A_F * spl_C_X(Ψ_A) / 2 * U_A^2 + Y_wind = ρ_air * A_L * spl_C_Y(Ψ_A) / 2 * U_A^2 + N_wind = ρ_air * A_L * L_pp * spl_C_N(Ψ_A) / 2 * U_A^2 + + dX[1] = du = ((X_H + X_R + X_P + X_wind) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) + dX[2] = + dv = + ( + (x_G^2) * (m^2) * u * r - (N_H + N_R + N_wind) * x_G * m + + ((Y_H + Y_R + Y_wind) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) + ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2)) + dX[3] = dr = (N_H + N_R + N_wind - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m) + dX[4] = dΨ = r + dX[5] = dδ = derivative(spl_δ, t) + dX[6] = dn_p = derivative(spl_n_p, t) + end + + R_0_dash_start = 0.022 + X_vv_dash_start = -0.040 + X_vr_dash_start = 0.002 + X_rr_dash_start = 0.011 + X_vvvv_dash_start = 0.771 + Y_v_dash_start = -0.315 + Y_r_dash_start = 0.083 + Y_vvv_dash_start = -1.607 + Y_vvr_dash_start = 0.379 + Y_vrr_dash_start = -0.391 + Y_rrr_dash_start = 0.008 + N_v_dash_start = -0.137 + N_r_dash_start = -0.049 + N_vvv_dash_start = -0.030 + N_vvr_dash_start = -0.294 + N_vrr_dash_start = 0.055 + N_rrr_dash_start = -0.013 + + p = [ + R_0_dash_start, + X_vv_dash_start, + X_vr_dash_start, + X_rr_dash_start, + X_vvvv_dash_start, + Y_v_dash_start, + Y_r_dash_start, + Y_vvv_dash_start, + Y_vvr_dash_start, + Y_vrr_dash_start, + Y_rrr_dash_start, + N_v_dash_start, + N_r_dash_start, + N_vvv_dash_start, + N_vvr_dash_start, + N_vrr_dash_start, + N_rrr_dash_start, + ] + + u0 = 2.29 * 0.512 + v0 = 0.0 + r0 = 0.0 + X0 = [u_obs[1]; v_obs[1]; r_obs[1]; Ψ_obs[1]; δ_obs[1]; n_p_obs[1]] + prob1 = ODEProblem(MMG!, X0, (time_obs[1], time_obs[end]), p) + + # create probabilistic model + Turing.@model function fitMMG(time_obs, obs, prob1) + σ_u ~ σ_u_prior_dist + σ_v ~ σ_v_prior_dist + σ_r ~ σ_r_prior_dist + R_0_dash ~ R_0_dash_prior_dist + X_vv_dash ~ X_vv_dash_prior_dist + X_vr_dash ~ X_vr_dash_prior_dist + X_rr_dash ~ X_rr_dash_prior_dist + X_vvvv_dash ~ X_vvvv_dash_prior_dist + Y_v_dash ~ Y_v_dash_prior_dist + Y_r_dash ~ Y_r_dash_prior_dist + Y_vvv_dash ~ Y_vvv_dash_prior_dist + Y_vvr_dash ~ Y_vvr_dash_prior_dist + Y_vrr_dash ~ Y_vrr_dash_prior_dist + Y_rrr_dash ~ Y_rrr_dash_prior_dist + N_v_dash ~ N_v_dash_prior_dist + N_r_dash ~ N_r_dash_prior_dist + N_vvv_dash ~ N_vvv_dash_prior_dist + N_vvr_dash ~ N_vvr_dash_prior_dist + N_vrr_dash ~ N_vrr_dash_prior_dist + N_rrr_dash ~ N_rrr_dash_prior_dist + + p = [ + R_0_dash, + X_vv_dash, + X_vr_dash, + X_rr_dash, + X_vvvv_dash, + Y_v_dash, + Y_r_dash, + Y_vvv_dash, + Y_vvr_dash, + Y_vrr_dash, + Y_rrr_dash, + N_v_dash, + N_r_dash, + N_vvv_dash, + N_vvr_dash, + N_vrr_dash, + N_rrr_dash, + ] + prob = remake(prob1, p=p) + sol = solve(prob, solver, abstol=abstol, reltol=reltol) + predicted = sol(time_obs) + for i in eachindex(predicted) + obs[1][i] ~ Normal(predicted[i][1], σ_u) # u + obs[2][i] ~ Normal(predicted[i][2], σ_v) # v + obs[3][i] ~ Normal(predicted[i][3], σ_r) # r + end + end + + return fitMMG(time_obs, [u_obs, v_obs, r_obs], prob1) +end \ No newline at end of file diff --git a/test/runtests.jl b/test/runtests.jl index 7e38e37..a4b68f5 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -6,4 +6,5 @@ using Distributions include("test_data.jl") include("test_kt.jl") include("test_mmg.jl") + include("test_mmg_wind.jl") end diff --git a/test/test_mmg.jl b/test/test_mmg.jl index fe2fa9c..b62026f 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -1,31 +1,21 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() -wind_force_and_moment_params = get_example_ship_wind_force_moment_params() @testset "mmg.jl KVLCC2_L7 turning" begin + duration = 200 # [s] max_δ_rad = 35 * pi / 180.0 # [rad] n_const = 17.95 # [rps] - μ_u_wind = 8.0 # [m/s] - μ_ψ_wind = 0.0 # [rad] - σ_u_wind = 1.0 # [m/s] - σ_ψ_wind = 0.1 # [rad] sampling = duration * 10 time_list = range(0.00, stop=duration, length=sampling) δ_rad_list = max_δ_rad .* ones(Float64, sampling) n_p_list = n_const .* ones(Float64, sampling) - u_wind_list = rand(Normal(μ_u_wind, σ_u_wind), length(time_list)) - ψ_wind_list = rand(Normal(μ_ψ_wind, σ_ψ_wind), length(time_list)) - mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, - wind_force_and_moment_params, time_list, δ_rad_list, n_p_list, - u_wind_list, - ψ_wind_list, u0=2.29 * 0.512, v0=0.0, r0=0.0, @@ -37,28 +27,19 @@ end @testset "mmg_zigzag_test" begin target_δ_rad = 20.0 * π / 180.0 target_ψ_rad_deviation = 20.0 * π / 180.0 - μ_u_wind = 8.0 # [m/s] - μ_ψ_wind = 0.0 # [rad] - σ_u_wind = 1.0 # [m/s] - σ_ψ_wind = 0.1 # [rad] start_time_second = 0.00 time_second_interval = 0.01 end_time_second = 80.00 time_list = start_time_second:time_second_interval:end_time_second n_const = 17.95 # [rps] n_p_list = n_const * ones(Float64, length(time_list)) - u_wind_list = rand(Normal(μ_u_wind, σ_u_wind), length(time_list)) - ψ_wind_list = rand(Normal(μ_ψ_wind, σ_ψ_wind), length(time_list)) u_list, v_list, r_list, x_list, y_list, ψ_list, δ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, - wind_force_and_moment_params, time_list, n_p_list, target_δ_rad, target_ψ_rad_deviation, - u_wind_list, - ψ_wind_list, ) end @@ -66,26 +47,17 @@ end duration = 200 # [s] max_δ_rad = 35 * pi / 180.0 # [rad] n_const = 17.95 # [rps] - μ_u_wind = 8.0 # [m/s] - μ_ψ_wind = 0.0 # [rad] - σ_u_wind = 1.0 # [m/s] - σ_ψ_wind = 0.1 # [rad] sampling = duration * 10 + 1 time_list = range(0.00, stop=duration, length=sampling) δ_rad_list = max_δ_rad .* ones(Float64, sampling) n_p_list = n_const .* ones(Float64, sampling) - u_wind_list = rand(Normal(μ_u_wind, σ_u_wind), length(time_list)) - ψ_wind_list = rand(Normal(μ_ψ_wind, σ_ψ_wind), length(time_list)) mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, - wind_force_and_moment_params, time_list, δ_rad_list, n_p_list, - u_wind_list, - ψ_wind_list, u0=2.29 * 0.512, v0=0.0, r0=0.0, @@ -127,34 +99,19 @@ end X_vv_dash_prior_dist=Uniform(-0.04 - parameter_width, -0.04 + parameter_width), X_vr_dash_prior_dist=Uniform(0.002 - parameter_width, 0.002 + parameter_width), X_rr_dash_prior_dist=Uniform(0.011 - parameter_width, 0.011 + parameter_width), - X_vvvv_dash_prior_dist=Uniform( - 0.771 - parameter_width, - 0.771 + parameter_width, - ), + X_vvvv_dash_prior_dist=Uniform(0.771 - parameter_width, 0.771 + parameter_width), Y_v_dash_prior_dist=Uniform(-0.315 - parameter_width, -0.315 + parameter_width), Y_r_dash_prior_dist=Uniform(0.083 - parameter_width, 0.083 + parameter_width), - Y_vvv_dash_prior_dist=Uniform( - -1.607 - parameter_width, - -1.607 + parameter_width, - ), + Y_vvv_dash_prior_dist=Uniform(-1.607 - parameter_width, -1.607 + parameter_width), Y_vvr_dash_prior_dist=Uniform(0.379 - parameter_width, 0.379 + parameter_width), - Y_vrr_dash_prior_dist=Uniform( - -0.391 - parameter_width, - -0.391 + parameter_width, - ), + Y_vrr_dash_prior_dist=Uniform(-0.391 - parameter_width, -0.391 + parameter_width), Y_rrr_dash_prior_dist=Uniform(0.008 - parameter_width, 0.008 + parameter_width), N_v_dash_prior_dist=Uniform(-0.137 - parameter_width, -0.137 + parameter_width), N_r_dash_prior_dist=Uniform(-0.049 - parameter_width, -0.049 + parameter_width), N_vvv_dash_prior_dist=Uniform(-0.03 - parameter_width, -0.03 + parameter_width), - N_vvr_dash_prior_dist=Uniform( - -0.294 - parameter_width, - -0.294 + parameter_width, - ), + N_vvr_dash_prior_dist=Uniform(-0.294 - parameter_width, -0.294 + parameter_width), N_vrr_dash_prior_dist=Uniform(0.055 - parameter_width, 0.055 + parameter_width), - N_rrr_dash_prior_dist=Uniform( - -0.013 - parameter_width, - -0.013 + parameter_width, - ), + N_rrr_dash_prior_dist=Uniform(-0.013 - parameter_width, -0.013 + parameter_width), ) chain = nuts_sampling_single_thread(model, n_samples, n_chains) end diff --git a/test/test_mmg_wind.jl b/test/test_mmg_wind.jl new file mode 100644 index 0000000..a06fef0 --- /dev/null +++ b/test/test_mmg_wind.jl @@ -0,0 +1,163 @@ +basic_params, maneuvering_params = get_KVLCC2_L7_params() +wind_force_and_moment_params = get_example_ship_wind_force_moment_params() + +@testset "mmg.jl KVLCC2_L7 turning with wind" begin + duration = 200 # [s] + max_δ_rad = 35 * pi / 180.0 # [rad] + n_const = 17.95 # [rps] + μ_U_W = 8.0 # [m/s] + μ_ψ_W = 0.0 # [rad] + σ_U_W = 1.0 # [m/s] + σ_ψ_W = 0.1 # [rad] + + sampling = duration * 10 + time_list = range(0.00, stop=duration, length=sampling) + δ_rad_list = max_δ_rad .* ones(Float64, sampling) + n_p_list = n_const .* ones(Float64, sampling) + U_W_list = rand(Normal(μ_U_W, σ_U_W), length(time_list)) + ψ_W_list = rand(Normal(μ_ψ_W, σ_ψ_W), length(time_list)) + + mmg_results = mmg_3dof_simulate( + basic_params, + maneuvering_params, + wind_force_and_moment_params, + time_list, + δ_rad_list, + n_p_list, + U_W_list, + ψ_W_list, + u0=2.29 * 0.512, + v0=0.0, + r0=0.0, + ) + u, v, r, x, y, ψ, δ, n_p = mmg_results + x_est, y_est, ψ_est = calc_position(time_list, u, v, r) +end + +@testset "mmg_zigzag_test with wind" begin + target_δ_rad = 20.0 * π / 180.0 + target_ψ_rad_deviation = 20.0 * π / 180.0 + μ_U_W = 8.0 # [m/s] + μ_ψ_W = 0.0 # [rad] + σ_U_W = 1.0 # [m/s] + σ_ψ_W = 0.1 # [rad] + start_time_second = 0.00 + time_second_interval = 0.01 + end_time_second = 80.00 + time_list = start_time_second:time_second_interval:end_time_second + n_const = 17.95 # [rps] + n_p_list = n_const * ones(Float64, length(time_list)) + U_W_list = rand(Normal(μ_U_W, σ_U_W), length(time_list)) + ψ_W_list = rand(Normal(μ_ψ_W, σ_ψ_W), length(time_list)) + u_list, v_list, r_list, x_list, y_list, ψ_list, δ_list = mmg_3dof_zigzag_test( + basic_params, + maneuvering_params, + wind_force_and_moment_params, + time_list, + n_p_list, + target_δ_rad, + target_ψ_rad_deviation, + U_W_list, + ψ_W_list, + ) +end + +@testset "mmg mcmc sampling func with wind" begin + duration = 200 # [s] + max_δ_rad = 35 * pi / 180.0 # [rad] + n_const = 17.95 # [rps] + μ_U_W = 8.0 # [m/s] + μ_ψ_W = 0.0 # [rad] + σ_U_W = 1.0 # [m/s] + σ_ψ_W = 0.1 # [rad] + + sampling = duration * 10 + 1 + time_list = range(0.00, stop=duration, length=sampling) + δ_rad_list = max_δ_rad .* ones(Float64, sampling) + n_p_list = n_const .* ones(Float64, sampling) + U_W_list = rand(Normal(μ_U_W, σ_U_W), length(time_list)) + ψ_W_list = rand(Normal(μ_ψ_W, σ_ψ_W), length(time_list)) + mmg_results = mmg_3dof_simulate( + basic_params, + maneuvering_params, + wind_force_and_moment_params, + time_list, + δ_rad_list, + n_p_list, + U_W_list, + ψ_W_list, + u0=2.29 * 0.512, + v0=0.0, + r0=0.0, + ) + u, v, r, δ, n_p = mmg_results + sampling_rate = 10 + time_obs = time_list[1:sampling_rate:end] + noize_dist = Normal(0.0, 0.01) + u_obs = u + rand(noize_dist, size(u)) + v_obs = v + rand(noize_dist, size(v)) + r_obs = r + rand(noize_dist, size(r)) + x, y, ψ = calc_position(time_obs, u_obs, v_obs, r_obs) + data = ShipDataAdvanced( + time_obs, + u_obs, + v_obs, + r_obs, + x, + y, + ψ, + δ_rad_list[1:sampling_rate:end], + n_p_list[1:sampling_rate:end], + U_W_list[1:sampling_rate:end], + ψ_W_list[1:sampling_rate:end], + ) + + parameter_width = 0.001 + n_samples = 10 + n_chains = 1 + model = create_model_for_mcmc_sample_mmg( + data, + basic_params, + wind_force_and_moment_params, + maneuvering_params.k_0, + maneuvering_params.k_1, + maneuvering_params.k_2, + ρ=1025.0, + σ_u_prior_dist=Uniform(0.00, 0.20), + σ_v_prior_dist=Uniform(0.00, 0.20), + σ_r_prior_dist=Uniform(0.00, 0.20), + R_0_dash_prior_dist=Uniform(0.022 - parameter_width, 0.022 + parameter_width), + X_vv_dash_prior_dist=Uniform(-0.04 - parameter_width, -0.04 + parameter_width), + X_vr_dash_prior_dist=Uniform(0.002 - parameter_width, 0.002 + parameter_width), + X_rr_dash_prior_dist=Uniform(0.011 - parameter_width, 0.011 + parameter_width), + X_vvvv_dash_prior_dist=Uniform( + 0.771 - parameter_width, + 0.771 + parameter_width, + ), + Y_v_dash_prior_dist=Uniform(-0.315 - parameter_width, -0.315 + parameter_width), + Y_r_dash_prior_dist=Uniform(0.083 - parameter_width, 0.083 + parameter_width), + Y_vvv_dash_prior_dist=Uniform( + -1.607 - parameter_width, + -1.607 + parameter_width, + ), + Y_vvr_dash_prior_dist=Uniform(0.379 - parameter_width, 0.379 + parameter_width), + Y_vrr_dash_prior_dist=Uniform( + -0.391 - parameter_width, + -0.391 + parameter_width, + ), + Y_rrr_dash_prior_dist=Uniform(0.008 - parameter_width, 0.008 + parameter_width), + N_v_dash_prior_dist=Uniform(-0.137 - parameter_width, -0.137 + parameter_width), + N_r_dash_prior_dist=Uniform(-0.049 - parameter_width, -0.049 + parameter_width), + N_vvv_dash_prior_dist=Uniform(-0.03 - parameter_width, -0.03 + parameter_width), + N_vvr_dash_prior_dist=Uniform( + -0.294 - parameter_width, + -0.294 + parameter_width, + ), + N_vrr_dash_prior_dist=Uniform(0.055 - parameter_width, 0.055 + parameter_width), + N_rrr_dash_prior_dist=Uniform( + -0.013 - parameter_width, + -0.013 + parameter_width, + ), + ) + chain = nuts_sampling_single_thread(model, n_samples, n_chains) +end