From 17e99f7bd354814a4e17f770c380c2e9ab066923 Mon Sep 17 00:00:00 2001 From: hinata235 <73246922+hinata235@users.noreply.github.com> Date: Wed, 12 Jul 2023 14:26:08 +0900 Subject: [PATCH 01/12] Add files via upload Addition of wind force term. --- src/ShipMMG.jl | 5 +- src/data.jl | 32 ++++- src/mmg.jl | 328 +++++++++++++++++++++++++++++++++++++++++-------- 3 files changed, 310 insertions(+), 55 deletions(-) diff --git a/src/ShipMMG.jl b/src/ShipMMG.jl index 0cd052b..e1de1b2 100644 --- a/src/ShipMMG.jl +++ b/src/ShipMMG.jl @@ -13,6 +13,7 @@ export calc_position, ShipData, get_KVLCC2_L7_basic_params, get_KVLCC2_L7_maneuvering_params, + get_structure_params, get_KVLCC2_L7_params, nuts_sampling_single_thread, nuts_sampling_multi_threads @@ -28,12 +29,14 @@ export kt_simulate, include("mmg.jl") export Mmg3DofBasicParams, Mmg3DofManeuveringParams, + Mmg3DofStructureParams, 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 + create_model_for_mcmc_sample_mmg, + wind_force_and_moment_coefficients include("draw.jl") export draw_gif_result, calc_position diff --git a/src/data.jl b/src/data.jl index 383bd4d..f4cbb87 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} +@with_kw struct ShipData{Tt,Tu,Tv,Tr,Tx,Ty,Tψ,Tδ,Tn_p,Tu_wind,Tψ_wind} time::Tt u::Tu v::Tv @@ -8,6 +8,8 @@ ψ::Tψ δ::Tδ n_p::Tn_p + u_wind::Tu_wind + ψ_wind::Tψ_wind end function get_KVLCC2_L7_basic_params(ρ = 1025.0) @@ -138,6 +140,34 @@ function get_KVLCC2_L7_maneuvering_params() maneuvering_params end +function get_structure_params() + L_pp = 7.00 # 船長Lpp[m] + B = 1.27 # 船幅[m] + d = 0.46 # 喫水[m] + D = 0.6563 # 深さ[m] + A_OD = 0.65 # デッキ上の構造物の側面投影面積[m^2] + H_BR = 0.85 # 喫水からブリッジ主要構造物の最高位[m] + H_C = 0.235 # 喫水から側面積中心までの高さ[m] + C = 0.0 # 船体中心から側面積中心までの前後方向座標(船首方向を正)[m] + + A_OD = A_OD # デッキ上の構造物の側面投影面積[m^2] + A_F = (D - d) * B # 船体の正面投影面積[m^2] + A_L = (D - d) * L_pp # 船体の側面投影面積[m^2] + H_BR = H_BR # 喫水からブリッジ主要構造物の最高位[m] + H_C = H_C # 喫水から側面積中心までの高さ[m] + C = C # 船体中心から側面積中心までの前後方向座標[m] + + structure_params = Mmg3DofStructureParams( + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, + ) + structure_params +end + function get_KVLCC2_L7_params() basic_params = get_KVLCC2_L7_basic_params() maneuvering_params = get_KVLCC2_L7_maneuvering_params() diff --git a/src/mmg.jl b/src/mmg.jl index 5d74f76..f5bd8be 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -1,11 +1,102 @@ +""" + wind_force_and_moment_coefficients(ψ,ψ_wind,p) + +conpute the parameter C_X,C_Y,C_N + +# Arguments +-`ψ`:ship angle +-`ψ_wind`:wind direction[deg] +- L_pp +- B +- A_OD +- A_F +- A_L +- H_BR +- H_C +- C +""" + +function wind_force_and_moment_coefficients( + ψ, + ψ_wind, + L_pp, + B, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C) + + + #ラジアンの調整 + ψ %= 2pi + + if ψ < 0 + ψ += 2pi + end + + #風の船体に対する流入角ψ_Aの計算 + ψ_A = pi/2 + ψ - deg2rad(ψ_wind) + + if ψ_A < 0 + ψ_A += 2pi + elseif ψ_A > 2pi + ψ_A -= 2pi + end + + #C_LF1の場合で調整 + C_CF = 0.404 + 0.368 * A_F / (B * H_BR) + 0.902 * H_BR / L_pp + + if deg2rad(0) <= ψ_A <= deg2rad(90) + + C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp + C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) + C_ALF = -0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp + C_YLI = pi * A_L / L_pp^2 + 0.116 +3.345 * A_F / (L_pp * B) + + elseif deg2rad(90) < ψ_A <= deg2rad(180) + + C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp -3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 + C_XLI = -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) + C_ALF = -0.314 - 1.117 * A_OD / A_L + C_YLI = pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp ^2 + + elseif deg2rad(180) < ψ_A <= deg2rad(270) + + C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp -3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 + C_XLI = -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) + C_ALF = -(-0.314 - 1.117 * A_OD / A_L) + C_YLI = -(pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp ^2) + + elseif deg2rad(270) < ψ_A <= deg2rad(360) + + C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp + C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) + C_ALF = -(-0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp) + C_YLI = -(pi * A_L / L_pp^2 + 0.116 +3.345 * A_F / (L_pp * B)) + + end + + + C_X = C_LF * cos(ψ_A) + C_XLI * (sin(ψ_A) - sin(ψ_A) * cos(ψ_A)^2 / 2) * sin(ψ_A) * cos(ψ_A) + C_ALF * sin(ψ_A) * cos(ψ_A)^3 + C_Y = C_CF * sin(ψ_A)^2 + C_YLI * (cos(ψ_A) + sin(ψ_A)^2 * cos(ψ_A) / 2) * sin(ψ_A) * cos(ψ_A) + C_N = C_Y * (0.297 * C / L_pp - 0.149 * (ψ_A - deg2rad(90))) + + + C_X,C_Y,C_N + +end + + """ mmg_3dof_model!(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`]. +- `dX`: [du, dv, dr, dx, dy, dψ, dδ, dn_p, du_wind, dψ_wind] +- `X`: the initial state values. [`u`, `v`, `r`, `x`, `y`, `ψ`, `δ`, `n_p`,`u_wind`,`ψ_wind`]. - `p`: ρ and the basic & maneuvering parameters and δ & n_p spline info. - ρ - L_pp @@ -33,6 +124,12 @@ MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`. - t_P - w_P0 - x_P + - A_OD + - A_F + - A_L + - H_BR + - H_C + - C - k_0 - k_1 - k_2 @@ -55,10 +152,12 @@ MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`. - N_rrr_dash - 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 = X + u, v, r, x, y, ψ, δ, n_p, u_wind, ψ_wind = X ρ, L_pp, B, @@ -85,6 +184,12 @@ function mmg_3dof_model!(dX, X, p, t) t_P, w_P0, x_P, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, k_0, k_1, k_2, @@ -106,7 +211,9 @@ function mmg_3dof_model!(dX, X, p, t) N_vrr_dash, N_rrr_dash, spl_δ, - spl_n_p = p + spl_n_p, + spl_u_wind, + spl_ψ_wind = p U = sqrt(u^2 + (v - r * x_G)^2) @@ -214,19 +321,43 @@ function mmg_3dof_model!(dX, X, p, t) ) ) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) - dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x) + + + + C_X_wind,C_Y_wind,C_N_wind = wind_force_and_moment_coefficients( + ψ, + ψ_wind, + L_pp, + B, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C + ) + + ρ_air = 1.225 + X_wind = ρ_air * A_F * C_X_wind / 2 * u_wind^2 + Y_wind = ρ_air * A_L * C_Y_wind / 2 * u_wind^2 + N_wind = ρ_air * A_L * L_pp * C_N_wind / 2 * u_wind^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) * x_G * m + - ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m) + (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 - 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[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) + dX[9] = du_wind = derivative(spl_u_wind, t) + dX[10] = dψ_wind = derivative(spl_ψ_wind, t) end """ @@ -336,23 +467,46 @@ Maneuvering parameters of target ship for MMG 3DOF simulation. end """ - 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 +Sturucture parameters of target ship for MMG 3DOF simulation. + +# Arguments +- `A_OD::T` +- `A_F::T` +- `A_L::T` +- `H_BR::T` +- `H_C::T` +- `C::T` +""" +@with_kw mutable struct Mmg3DofStructureParams{T} + A_OD::T + A_F::T + A_L::T + H_BR::T + H_C::T + C::T +end + +""" + mmg_3dof_simulate(time_list, n_p_list, δ_list, u_wind_list, ψ_wind_list basic_params, maneuvering_params,structure_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. +- `structure_params::Mmg3DofStructureParams,` : 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. +- `u_wind_list` :the list of wind velocity [m/s]. +- `ψ_wind_list` :the list of wind direction [deg]. - `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() @@ -370,12 +524,16 @@ 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_wind_list = max_δ_rad .* ones(Float64, sampling); +julia> ψ_wind_list = n_const .* ones(Float64, sampling); julia> mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, time_list, δ_rad_list, n_p_list, + u_wind_list, + ψ_wind_list, u0 = 2.29 * 0.512, v0 = 0.0, r0 = 0.0, @@ -385,15 +543,18 @@ julia> mmg_results = mmg_3dof_simulate( function mmg_3dof_simulate( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, + structure_params::Mmg3DofStructureParams, time_list, δ_list, - n_p_list; + n_p_list, + u_wind_list, + ψ_wind_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, @@ -445,6 +606,14 @@ function mmg_3dof_simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash = maneuvering_params + + @unpack A_OD, + A_F, + A_L, + H_BR, + H_C, + C = structure_params + simulate( L_pp, B, @@ -471,6 +640,12 @@ function mmg_3dof_simulate( t_P, w_P0, x_P, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, k_0, k_1, k_2, @@ -494,12 +669,14 @@ function mmg_3dof_simulate( time_list, δ_list, n_p_list, + u_wind_list, + ψ_wind_list, u0=u0, v0=v0, r0=r0, x0=x0, y0=y0, - Ψ0=Ψ0, + ψ0=ψ0, ρ=ρ, algorithm=algorithm, reltol=reltol, @@ -508,9 +685,9 @@ function mmg_3dof_simulate( end """ - 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 + simulate(time_list, n_p_list, δ_list, u_wind_list, ψ_wind_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, [, 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 @@ -539,6 +716,12 @@ This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. - `t_P`: - `w_P0`: - `x_P`: +- `A_OD:` +- `A_F:` +- `A_L:` +- `H_BR:` +- `H_C:` +- `C:` - `k_0` - `k_1` - `k_2` @@ -562,12 +745,14 @@ This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. - `time_list`: the list of simulatino time. - `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. +- `u_wind_list` :the list of wind velocity [m/s]. +- `ψ_wind_list` :the list of wind direction [deg]. - `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() @@ -599,6 +784,12 @@ function simulate( t_P, w_P0, x_P, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, k_0, k_1, k_2, @@ -621,13 +812,15 @@ function simulate( N_rrr_dash, time_list, δ_list, - n_p_list; + n_p_list, + u_wind_list, + ψ_wind_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, @@ -635,8 +828,12 @@ function simulate( ) spl_δ = Spline1D(time_list, δ_list) spl_n_p = Spline1D(time_list, n_p_list) + + spl_u_wind = Spline1D(time_list, u_wind_list) + spl_ψ_wind = Spline1D(time_list, ψ_wind_list) - X0 = [u0; v0; r0; x0; y0; Ψ0; δ_list[1]; n_p_list[1]] + + X0 = [u0; v0; r0; x0; y0; ψ0; δ_list[1]; n_p_list[1]; u_wind_list[1]; ψ_wind_list[1]] p = [ ρ, L_pp, @@ -664,6 +861,12 @@ function simulate( t_P, w_P0, x_P, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, k_0, k_1, k_2, @@ -686,6 +889,8 @@ function simulate( N_rrr_dash, spl_δ, spl_n_p, + spl_u_wind, + spl_ψ_wind, ] prob = ODEProblem(mmg_3dof_model!, X0, (time_list[1], time_list[end]), p) sol = solve(prob, algorithm, reltol=reltol, abstol=abstol) @@ -696,30 +901,34 @@ function simulate( r = results[3, :] x = results[4, :] y = results[5, :] - Ψ = results[6, :] + ψ = results[6, :] δ = results[7, :] n_p = results[8, :] - u, v, r, x, y, Ψ, δ, n_p + u_wind = results[9, :] + ψ_wind = results[10, :] + u, v, r, x, y, ψ, δ, n_p, u_wind, ψ_wind end """ - 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, Ψ, δ + mmg_3dof_zigzag_test(basic_params, maneuvering_params,structure_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, ψ, δ 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. +- `structure_params::Mmg3DofStructureParams,` : 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. +- `u_wind_list` :the list of wind velocity [m/s]. +- `ψ_wind_list` :the list of wind direction [deg]. - `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]. @@ -734,36 +943,43 @@ 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> δ_list, u_list, v_list, r_list, Ψ_list = mmg_3dof_zigzag_test( +julia> u_wind_list = max_δ_rad .* ones(Float64, sampling); +julia> ψ_wind_list = n_const .* ones(Float64, sampling); +julia> δ_list, u_list, v_list, r_list, ψ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, 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, + structure_params::Mmg3DofStructureParams, time_list, n_p_list, target_δ_rad, - target_Ψ_rad_deviation; + target_ψ_rad_deviation, + u_wind_list, + ψ_wind_list; 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, @@ -771,7 +987,7 @@ function mmg_3dof_zigzag_test( reltol=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)) @@ -779,11 +995,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 @@ -823,18 +1039,21 @@ 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, + structure_params, time_list[start_index:end], δ_list, n_p_list[start_index:end], + u_wind_list[start_index:end], + ψ_wind_list[start_index:end]; u0=u0, v0=v0, r0=r0, x0=x0, y0=y0, - Ψ0=Ψ, + ψ0=ψ, ρ=ρ, algorithm=algorithm, reltol=reltol, @@ -842,13 +1061,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) @@ -858,9 +1077,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] @@ -868,7 +1087,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, @@ -876,7 +1095,7 @@ function mmg_3dof_zigzag_test( final_r_list, final_x_list, final_y_list, - final_Ψ_list, + final_ψ_list, final_δ_list end @@ -915,6 +1134,7 @@ function create_model_for_mcmc_sample_mmg( u_obs = data.u v_obs = data.v r_obs = data.r + ψ_obs = data.ψ δ_obs = data.δ n_p_obs = data.n_p @@ -948,7 +1168,7 @@ function create_model_for_mcmc_sample_mmg( spl_δ = Spline1D(time_obs, δ_obs) spl_n_p = Spline1D(time_obs, n_p_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, @@ -1073,9 +1293,11 @@ function create_model_for_mcmc_sample_mmg( ) ) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) + + ρ_air = 1.225 + 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 = + dX[2] = dv = ( (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) @@ -1126,7 +1348,7 @@ 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]; n_p_obs[1]] + 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 @@ -1174,7 +1396,7 @@ function create_model_for_mcmc_sample_mmg( prob = remake(prob1, p=p) sol = solve(prob, solver, abstol=abstol, reltol=reltol) predicted = sol(time_obs) - for i = 1:length(predicted) + 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 @@ -1182,4 +1404,4 @@ function create_model_for_mcmc_sample_mmg( end return fitMMG(time_obs, [u_obs, v_obs, r_obs], prob1) -end +end \ No newline at end of file From d020b9da6121fa19ada67e480cf19107f48b993b Mon Sep 17 00:00:00 2001 From: hinata235 <73246922+hinata235@users.noreply.github.com> Date: Wed, 12 Jul 2023 14:26:59 +0900 Subject: [PATCH 02/12] Add files via upload Addition of wind force terms. --- test/test_mmg.jl | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/test/test_mmg.jl b/test/test_mmg.jl index b62026f..33a1ec8 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -10,12 +10,17 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() 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(8,1),length(time_list)) + ψ_wind_list = rand(Normal(0,0.1)length(time_list)) + mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, time_list, δ_rad_list, n_p_list, + u_wind_list, + ψ_wind_list, u0=2.29 * 0.512, v0=0.0, r0=0.0, @@ -33,6 +38,8 @@ end 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(8,1),length(time_list)) + ψ_wind_list = rand(Normal(0,0.1)length(time_list)) u_list, v_list, r_list, x_list, y_list, ψ_list, δ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, @@ -40,6 +47,8 @@ end n_p_list, target_δ_rad, target_ψ_rad_deviation, + u_wind_list, + ψ_wind_list, ) end From 6951846bc519c84d688bdf6f6628fba88d0c3483 Mon Sep 17 00:00:00 2001 From: hinata235 <73246922+hinata235@users.noreply.github.com> Date: Sat, 15 Jul 2023 22:14:02 +0900 Subject: [PATCH 03/12] Add files via upload Added structure_params to mmg_3dof_simulate and mmg_3dof_zigzag_test arguments. --- test/test_mmg.jl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/test_mmg.jl b/test/test_mmg.jl index 33a1ec8..5b8d41e 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -16,6 +16,7 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, + structure_params, time_list, δ_rad_list, n_p_list, @@ -43,6 +44,7 @@ end u_list, v_list, r_list, x_list, y_list, ψ_list, δ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, + structure_params, time_list, n_p_list, target_δ_rad, From c34247f4f9a31cc2e2ed9da0d0575179870eff6f Mon Sep 17 00:00:00 2001 From: hinata235 <73246922+hinata235@users.noreply.github.com> Date: Mon, 17 Jul 2023 11:49:48 +0900 Subject: [PATCH 04/12] Add files via upload MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Delete Tu_wind,Tψ_wind from ShipData. --- src/data.jl | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/data.jl b/src/data.jl index f4cbb87..6ccf093 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_wind,Tψ_wind} +@with_kw struct ShipData{Tt,Tu,Tv,Tr,Tx,Ty,Tψ,Tδ,Tn_p} time::Tt u::Tu v::Tv @@ -8,8 +8,6 @@ ψ::Tψ δ::Tδ n_p::Tn_p - u_wind::Tu_wind - ψ_wind::Tψ_wind end function get_KVLCC2_L7_basic_params(ρ = 1025.0) From cb60a61ff951ec63559a239eccd0ccd18407d8e0 Mon Sep 17 00:00:00 2001 From: hinata235 <73246922+hinata235@users.noreply.github.com> Date: Mon, 17 Jul 2023 11:52:17 +0900 Subject: [PATCH 05/12] Add files via upload Add wind variables and structure_params. --- test/test_mmg.jl | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/test/test_mmg.jl b/test/test_mmg.jl index 5b8d41e..0e959b9 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -1,17 +1,22 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() +structure_params = get_structure_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 # [deg] + σ_u_wind = 1.0 # [m/s] + σ_ψ_wind = 0.1 # [deg] 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(8,1),length(time_list)) - ψ_wind_list = rand(Normal(0,0.1)length(time_list)) + 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, @@ -33,14 +38,18 @@ 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 # [deg] + σ_u_wind = 1.0 # [m/s] + σ_ψ_wind = 0.1 # [deg] 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(8,1),length(time_list)) - ψ_wind_list = rand(Normal(0,0.1)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, @@ -58,17 +67,26 @@ 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 # [deg] + σ_u_wind = 1.0 # [m/s] + σ_ψ_wind = 0.1 # [deg] 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, + structure_params, time_list, δ_rad_list, n_p_list, + u_wind_list, + ψ_wind_list, u0=2.29 * 0.512, v0=0.0, r0=0.0, From 8a55b11605f7b122fba4055cd3d418e4102c7f06 Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 13:48:34 +0900 Subject: [PATCH 06/12] add JuliaFormatter setting file --- .JuliaFormatter.toml | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 .JuliaFormatter.toml diff --git a/.JuliaFormatter.toml b/.JuliaFormatter.toml new file mode 100644 index 0000000..a672d2e --- /dev/null +++ b/.JuliaFormatter.toml @@ -0,0 +1,7 @@ +remove_extra_newlines=true +join_lines_based_on_source=true +whitespace_in_kwargs=false +short_to_long_function_def=true +always_for_in=true +verbose=true +margin=88 \ No newline at end of file From 51a69705e85a312ba77ee00a21d6e27b1691bf93 Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 14:11:21 +0900 Subject: [PATCH 07/12] julia formatter --- src/data.jl | 18 ++++++++-------- src/kt.jl | 13 ++++++------ src/mmg.jl | 53 ++++++++++++++++++++++++----------------------- test/runtests.jl | 2 -- test/test_data.jl | 2 +- test/test_mmg.jl | 38 ++++++++++++++++++++++----------- 6 files changed, 69 insertions(+), 57 deletions(-) diff --git a/src/data.jl b/src/data.jl index 6ccf093..cff6d9e 100644 --- a/src/data.jl +++ b/src/data.jl @@ -10,7 +10,7 @@ n_p::Tn_p end -function get_KVLCC2_L7_basic_params(ρ = 1025.0) +function get_KVLCC2_L7_basic_params(ρ=1025.0) L_pp = 7.00 # 船長Lpp[m] B = 1.27 # 船幅[m] d = 0.46 # 喫水[m] @@ -154,7 +154,7 @@ function get_structure_params() H_BR = H_BR # 喫水からブリッジ主要構造物の最高位[m] H_C = H_C # 喫水から側面積中心までの高さ[m] C = C # 船体中心から側面積中心までの前後方向座標[m] - + structure_params = Mmg3DofStructureParams( A_OD, A_F, @@ -172,7 +172,7 @@ function get_KVLCC2_L7_params() basic_params, maneuvering_params end -function calc_position(time_vec, u_vec, v_vec, r_vec; x0 = 0.0, y0 = 0.0, ψ0 = 0.0) +function calc_position(time_vec, u_vec, v_vec, r_vec; x0=0.0, y0=0.0, ψ0=0.0) dim = length(time_vec) x_vec = zeros(Float64, dim) y_vec = zeros(Float64, dim) @@ -193,12 +193,12 @@ function nuts_sampling_single_thread( model, n_samples::Int, n_chains::Int; - target_acceptance::Float64 = 0.65, - progress = true, + target_acceptance::Float64=0.65, + progress=true ) sampler = NUTS(target_acceptance) mapreduce( - c -> sample(model, sampler, n_samples, progress = progress), + c -> sample(model, sampler, n_samples, progress=progress), chainscat, 1:n_chains, ) @@ -208,9 +208,9 @@ function nuts_sampling_multi_threads( model, n_samples::Int, n_chains::Int; - target_acceptance::Float64 = 0.65, - progress = false, + target_acceptance::Float64=0.65, + progress=false ) sampler = NUTS(target_acceptance) - sample(model, sampler, MCMCThreads(), n_samples, n_chains, progress = progress) + sample(model, sampler, MCMCThreads(), n_samples, n_chains, progress=progress) end diff --git a/src/kt.jl b/src/kt.jl index dbf8436..e90050f 100644 --- a/src/kt.jl +++ b/src/kt.jl @@ -85,9 +85,8 @@ function kt_simulate( Ψ0=0.0, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8 + abstol=1e-8, ) - spl_δ = Spline1D(time_list, δ_list) X0 = [u0; v0; r0; x0; y0; Ψ0; δ_list[1]] @@ -168,7 +167,7 @@ function kt_zigzag_test( δ_rad_rate=10.0 * π / 180, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8 + abstol=1e-8, ) target_Ψ_rad_deviation = abs(target_Ψ_rad_deviation) @@ -205,7 +204,7 @@ function kt_zigzag_test( y0 = final_y_list[start_index-1] end - for i = (start_index+1):length(time_list) + 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 @@ -306,7 +305,7 @@ function estimate_kt_lsm_time_window_sampling(data::ShipData, window_size::Int) n_samples = length(time_vec) - window_size K_samples = zeros(n_samples) T_samples = zeros(n_samples) - for i = 1:n_samples + for i in 1:n_samples K_samples[i], T_samples[i] = estimate_kt_lsm(r[i:i+window_size], dr[i:i+window_size], δ[i:i+window_size]) end @@ -317,7 +316,7 @@ function create_model_for_mcmc_sample_kt( data::ShipData; σ_r_prior_dist::Distribution=Chi(5), K_prior_dist::Distribution=Uniform(0.01, 10.0), - T_prior_dist::Distribution=truncated(Normal(100.0, 50.0), 10.0, 200.0) + T_prior_dist::Distribution=truncated(Normal(100.0, 50.0), 10.0, 200.0), ) time_obs = data.time r_obs = data.r @@ -347,7 +346,7 @@ function create_model_for_mcmc_sample_kt( prob = remake(prob1, p=p) sol = solve(prob, Tsit5()) predicted = sol(time_obs) - for i = 1:length(predicted) + for i in 1:length(predicted) r_obs[i] ~ Normal(predicted[i][1], σ_r) # index number of r is 1 end end diff --git a/src/mmg.jl b/src/mmg.jl index 40bed16..2a5c72d 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -26,7 +26,8 @@ function wind_force_and_moment_coefficients( A_L, H_BR, H_C, - C) + C +) #ラジアンの調整 @@ -37,7 +38,7 @@ function wind_force_and_moment_coefficients( end #風の船体に対する流入角ψ_Aの計算 - ψ_A = pi/2 + ψ - deg2rad(ψ_wind) + ψ_A = pi / 2 + ψ - deg2rad(ψ_wind) if ψ_A < 0 ψ_A += 2pi @@ -48,33 +49,33 @@ function wind_force_and_moment_coefficients( #C_LF1の場合で調整 C_CF = 0.404 + 0.368 * A_F / (B * H_BR) + 0.902 * H_BR / L_pp - if deg2rad(0) <= ψ_A <= deg2rad(90) + if deg2rad(0) <= ψ_A <= deg2rad(90) C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) C_ALF = -0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp - C_YLI = pi * A_L / L_pp^2 + 0.116 +3.345 * A_F / (L_pp * B) + C_YLI = pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B) elseif deg2rad(90) < ψ_A <= deg2rad(180) - C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp -3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 + C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 C_XLI = -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) C_ALF = -0.314 - 1.117 * A_OD / A_L - C_YLI = pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp ^2 + C_YLI = pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2 elseif deg2rad(180) < ψ_A <= deg2rad(270) - C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp -3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 + C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 C_XLI = -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) C_ALF = -(-0.314 - 1.117 * A_OD / A_L) - C_YLI = -(pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp ^2) + C_YLI = -(pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2) elseif deg2rad(270) < ψ_A <= deg2rad(360) - + C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) C_ALF = -(-0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp) - C_YLI = -(pi * A_L / L_pp^2 + 0.116 +3.345 * A_F / (L_pp * B)) + C_YLI = -(pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B)) end @@ -84,7 +85,7 @@ function wind_force_and_moment_coefficients( C_N = C_Y * (0.297 * C / L_pp - 0.149 * (ψ_A - deg2rad(90))) - C_X,C_Y,C_N + C_X, C_Y, C_N end @@ -322,9 +323,9 @@ function mmg_3dof_model!(dX, X, p, t) ) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) - - C_X_wind,C_Y_wind,C_N_wind = wind_force_and_moment_coefficients( + + C_X_wind, C_Y_wind, C_N_wind = wind_force_and_moment_coefficients( ψ, ψ_wind, L_pp, @@ -338,19 +339,19 @@ function mmg_3dof_model!(dX, X, p, t) ) ρ_air = 1.225 - X_wind = ρ_air * A_F * C_X_wind / 2 * u_wind^2 - Y_wind = ρ_air * A_L * C_Y_wind / 2 * u_wind^2 + X_wind = ρ_air * A_F * C_X_wind / 2 * u_wind^2 + Y_wind = ρ_air * A_L * C_Y_wind / 2 * u_wind^2 N_wind = ρ_air * A_L * L_pp * C_N_wind / 2 * u_wind^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 + 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) + (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[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 @@ -828,7 +829,7 @@ function simulate( ) spl_δ = Spline1D(time_list, δ_list) spl_n_p = Spline1D(time_list, n_p_list) - + spl_u_wind = Spline1D(time_list, u_wind_list) spl_ψ_wind = Spline1D(time_list, ψ_wind_list) @@ -1057,7 +1058,7 @@ function mmg_3dof_zigzag_test( ρ=ρ, algorithm=algorithm, reltol=reltol, - abstol=abstol, + abstol=abstol ) # get finish index @@ -1298,10 +1299,10 @@ function create_model_for_mcmc_sample_mmg( 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) * 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)) + ( + (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 - 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) @@ -1348,7 +1349,7 @@ 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]; δ_obs[1]; n_p_obs[1]] prob1 = ODEProblem(MMG!, X0, (time_obs[1], time_obs[end]), p) # create probabilistic model diff --git a/test/runtests.jl b/test/runtests.jl index 0e508a1..7e38e37 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -3,9 +3,7 @@ using Test using Distributions @testset "ShipMMG.jl" begin - include("test_data.jl") include("test_kt.jl") include("test_mmg.jl") - end diff --git a/test/test_data.jl b/test/test_data.jl index e8f2e2c..0cb8237 100644 --- a/test/test_data.jl +++ b/test/test_data.jl @@ -3,7 +3,7 @@ u_vec = [1.0, 1.0] v_vec = [0.0, 0.0] r_vec = [0.0, 0.0] - x, y, ψ = calc_position(time_vec, u_vec, v_vec, r_vec, x0 = 1.0, y0 = 1.0, ψ0 = 0.0) + x, y, ψ = calc_position(time_vec, u_vec, v_vec, r_vec, x0=1.0, y0=1.0, ψ0=0.0) @test x[2] ≈ 3.0 @test y[2] ≈ 1.0 @test ψ[2] ≈ 0.0 diff --git a/test/test_mmg.jl b/test/test_mmg.jl index 0e959b9..a664a16 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -2,7 +2,6 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() structure_params = get_structure_params() @testset "mmg.jl KVLCC2_L7 turning" begin - duration = 200 # [s] max_δ_rad = 35 * pi / 180.0 # [rad] n_const = 17.95 # [rps] @@ -15,8 +14,8 @@ structure_params = get_structure_params() 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)) + 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, @@ -48,8 +47,8 @@ end 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_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, @@ -76,8 +75,8 @@ end 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)) + 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, @@ -128,19 +127,34 @@ 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 From bebb5eb7be63a940713a743d018658a89795f995 Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 14:28:55 +0900 Subject: [PATCH 08/12] change deg to rad --- src/mmg.jl | 136 ++++++++++++++++++++++++++--------------------- test/test_mmg.jl | 12 ++--- 2 files changed, 82 insertions(+), 66 deletions(-) diff --git a/src/mmg.jl b/src/mmg.jl index 2a5c72d..7aaa2eb 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -1,11 +1,34 @@ +""" +Wind angle of attack - 0 [rad] point out to North and rotating clockwise positive angle. +""" +function get_wind_attack_of_angle( + ψ_ship, + ψ_wind, +) + ψ_ship %= 2pi + + if ψ_ship < 0 + ψ_ship += 2pi + end + + ψ_A = pi / 2 + ψ_ship - ψ_wind + + if ψ_A < 0 + ψ_A += 2pi + elseif ψ_A > 2pi + ψ_A -= 2pi + end + ψ_A +end + """ wind_force_and_moment_coefficients(ψ,ψ_wind,p) conpute the parameter C_X,C_Y,C_N # Arguments --`ψ`:ship angle --`ψ_wind`:wind direction[deg] +-`ψ`:ship angle [rad] +-`ψ_wind`:wind direction[rad] - L_pp - B - A_OD @@ -15,7 +38,6 @@ conpute the parameter C_X,C_Y,C_N - H_C - C """ - function wind_force_and_moment_coefficients( ψ, ψ_wind, @@ -26,70 +48,58 @@ function wind_force_and_moment_coefficients( A_L, H_BR, H_C, - C + C, ) - - - #ラジアンの調整 - ψ %= 2pi - - if ψ < 0 - ψ += 2pi - end - - #風の船体に対する流入角ψ_Aの計算 - ψ_A = pi / 2 + ψ - deg2rad(ψ_wind) - - if ψ_A < 0 - ψ_A += 2pi - elseif ψ_A > 2pi - ψ_A -= 2pi - end + ψ_A = get_wind_attack_of_angle(ψ, ψ_wind) #C_LF1の場合で調整 C_CF = 0.404 + 0.368 * A_F / (B * H_BR) + 0.902 * H_BR / L_pp if deg2rad(0) <= ψ_A <= deg2rad(90) - C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) C_ALF = -0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp C_YLI = pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B) elseif deg2rad(90) < ψ_A <= deg2rad(180) - - C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 - C_XLI = -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) + C_LF = + 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - + 0.341 * A_F / B^2 + C_XLI = + -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - + 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) C_ALF = -0.314 - 1.117 * A_OD / A_L C_YLI = pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2 elseif deg2rad(180) < ψ_A <= deg2rad(270) - - C_LF = 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - 0.341 * A_F / B^2 - C_XLI = -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) + C_LF = + 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - + 0.341 * A_F / B^2 + C_XLI = + -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - + 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) C_ALF = -(-0.314 - 1.117 * A_OD / A_L) C_YLI = -(pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2) elseif deg2rad(270) < ψ_A <= deg2rad(360) - C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) C_ALF = -(-0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp) C_YLI = -(pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B)) - end - - C_X = C_LF * cos(ψ_A) + C_XLI * (sin(ψ_A) - sin(ψ_A) * cos(ψ_A)^2 / 2) * sin(ψ_A) * cos(ψ_A) + C_ALF * sin(ψ_A) * cos(ψ_A)^3 - C_Y = C_CF * sin(ψ_A)^2 + C_YLI * (cos(ψ_A) + sin(ψ_A)^2 * cos(ψ_A) / 2) * sin(ψ_A) * cos(ψ_A) + C_X = + C_LF * cos(ψ_A) + + C_XLI * (sin(ψ_A) - sin(ψ_A) * cos(ψ_A)^2 / 2) * sin(ψ_A) * cos(ψ_A) + + C_ALF * sin(ψ_A) * cos(ψ_A)^3 + C_Y = + C_CF * sin(ψ_A)^2 + + C_YLI * (cos(ψ_A) + sin(ψ_A)^2 * cos(ψ_A) / 2) * sin(ψ_A) * cos(ψ_A) C_N = C_Y * (0.297 * C / L_pp - 0.149 * (ψ_A - deg2rad(90))) - C_X, C_Y, C_N - end - """ mmg_3dof_model!(dX, X, p, t) @@ -323,8 +333,6 @@ function mmg_3dof_model!(dX, X, p, t) ) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) - - C_X_wind, C_Y_wind, C_N_wind = wind_force_and_moment_coefficients( ψ, ψ_wind, @@ -335,7 +343,7 @@ function mmg_3dof_model!(dX, X, p, t) A_L, H_BR, H_C, - C + C, ) ρ_air = 1.225 @@ -343,15 +351,18 @@ function mmg_3dof_model!(dX, X, p, t) Y_wind = ρ_air * A_L * C_Y_wind / 2 * u_wind^2 N_wind = ρ_air * A_L * L_pp * C_N_wind / 2 * u_wind^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 + 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[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 @@ -501,7 +512,7 @@ This function has the same logic of `ShipMMG.simulate()`. - `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. - `u_wind_list` :the list of wind velocity [m/s]. -- `ψ_wind_list` :the list of wind direction [deg]. +- `ψ_wind_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]. @@ -559,7 +570,7 @@ function mmg_3dof_simulate( ρ=1025.0, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8 + abstol=1e-8, ) @unpack L_pp, B, @@ -747,7 +758,7 @@ This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. - `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. - `u_wind_list` :the list of wind velocity [m/s]. -- `ψ_wind_list` :the list of wind direction [deg]. +- `ψ_wind_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]. @@ -825,7 +836,7 @@ function simulate( ρ=1025.0, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8 + abstol=1e-8, ) spl_δ = Spline1D(time_list, δ_list) spl_n_p = Spline1D(time_list, n_p_list) @@ -833,8 +844,8 @@ function simulate( spl_u_wind = Spline1D(time_list, u_wind_list) spl_ψ_wind = Spline1D(time_list, ψ_wind_list) - - X0 = [u0; v0; r0; x0; y0; ψ0; δ_list[1]; n_p_list[1]; u_wind_list[1]; ψ_wind_list[1]] + X0 = + [u0; v0; r0; x0; y0; ψ0; δ_list[1]; n_p_list[1]; u_wind_list[1]; ψ_wind_list[1]] p = [ ρ, L_pp, @@ -923,7 +934,7 @@ Returns the MMG 3DOF zigzag simulation results. - `δ_list`: the list of rudder angle [rad]. - `n_p_list`: the list of propeller rps. - `u_wind_list` :the list of wind velocity [m/s]. -- `ψ_wind_list` :the list of wind direction [deg]. +- `ψ_wind_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]. @@ -986,7 +997,7 @@ function mmg_3dof_zigzag_test( ρ=1025.0, algorithm=Tsit5(), reltol=1e-8, - abstol=1e-8 + abstol=1e-8, ) target_ψ_rad_deviation = abs(target_ψ_rad_deviation) @@ -1023,7 +1034,7 @@ function mmg_3dof_zigzag_test( y0 = final_y_list[start_index-1] end - for i = (start_index+1):length(time_list) + 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 @@ -1058,7 +1069,7 @@ function mmg_3dof_zigzag_test( ρ=ρ, algorithm=algorithm, reltol=reltol, - abstol=abstol + abstol=abstol, ) # get finish index @@ -1129,7 +1140,7 @@ 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 @@ -1239,7 +1250,10 @@ 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) @@ -1297,12 +1311,14 @@ function create_model_for_mcmc_sample_mmg( ρ_air = 1.225 - 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) * 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[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) * 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 - 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) diff --git a/test/test_mmg.jl b/test/test_mmg.jl index a664a16..eba8082 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -6,9 +6,9 @@ structure_params = get_structure_params() max_δ_rad = 35 * pi / 180.0 # [rad] n_const = 17.95 # [rps] μ_u_wind = 8.0 # [m/s] - μ_ψ_wind = 0.0 # [deg] + μ_ψ_wind = 0.0 # [rad] σ_u_wind = 1.0 # [m/s] - σ_ψ_wind = 0.1 # [deg] + σ_ψ_wind = 0.1 # [rad] sampling = duration * 10 time_list = range(0.00, stop=duration, length=sampling) @@ -38,9 +38,9 @@ end target_δ_rad = 20.0 * π / 180.0 target_ψ_rad_deviation = 20.0 * π / 180.0 μ_u_wind = 8.0 # [m/s] - μ_ψ_wind = 0.0 # [deg] + μ_ψ_wind = 0.0 # [rad] σ_u_wind = 1.0 # [m/s] - σ_ψ_wind = 0.1 # [deg] + σ_ψ_wind = 0.1 # [rad] start_time_second = 0.00 time_second_interval = 0.01 end_time_second = 80.00 @@ -67,9 +67,9 @@ end max_δ_rad = 35 * pi / 180.0 # [rad] n_const = 17.95 # [rps] μ_u_wind = 8.0 # [m/s] - μ_ψ_wind = 0.0 # [deg] + μ_ψ_wind = 0.0 # [rad] σ_u_wind = 1.0 # [m/s] - σ_ψ_wind = 0.1 # [deg] + σ_ψ_wind = 0.1 # [rad] sampling = duration * 10 + 1 time_list = range(0.00, stop=duration, length=sampling) From 098f681d8a08558629d2b298fda212750e00ebbd Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 14:39:47 +0900 Subject: [PATCH 09/12] modify params of wind_force_and_moment_coefficients --- src/mmg.jl | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/mmg.jl b/src/mmg.jl index 7aaa2eb..b9a28be 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -27,8 +27,7 @@ end conpute the parameter C_X,C_Y,C_N # Arguments --`ψ`:ship angle [rad] --`ψ_wind`:wind direction[rad] +-`ψ_A`: Wind attack of angle [rad] - L_pp - B - A_OD @@ -39,8 +38,7 @@ conpute the parameter C_X,C_Y,C_N - C """ function wind_force_and_moment_coefficients( - ψ, - ψ_wind, + ψ_A, L_pp, B, A_OD, @@ -50,8 +48,6 @@ function wind_force_and_moment_coefficients( H_C, C, ) - ψ_A = get_wind_attack_of_angle(ψ, ψ_wind) - #C_LF1の場合で調整 C_CF = 0.404 + 0.368 * A_F / (B * H_BR) + 0.902 * H_BR / L_pp @@ -333,9 +329,9 @@ function mmg_3dof_model!(dX, X, p, t) ) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) + ψ_A = get_wind_attack_of_angle(ψ, ψ_wind) C_X_wind, C_Y_wind, C_N_wind = wind_force_and_moment_coefficients( - ψ, - ψ_wind, + ψ_A, L_pp, B, A_OD, From bfe64a69d74bfe78f57afb092d9e26a185611843 Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 16:01:29 +0900 Subject: [PATCH 10/12] move param estimation from mmg.jl to data.jl --- src/ShipMMG.jl | 1 + src/data.jl | 131 +++++++++++++++++++++++++++++++++++++++++++++++-- src/mmg.jl | 75 ---------------------------- 3 files changed, 127 insertions(+), 80 deletions(-) diff --git a/src/ShipMMG.jl b/src/ShipMMG.jl index d7e1d03..adf90e3 100644 --- a/src/ShipMMG.jl +++ b/src/ShipMMG.jl @@ -14,6 +14,7 @@ export calc_position, get_KVLCC2_L7_maneuvering_params, get_structure_params, get_KVLCC2_L7_params, + get_example_ship_wind_force_moment_params, nuts_sampling_single_thread, nuts_sampling_multi_threads diff --git a/src/data.jl b/src/data.jl index cff6d9e..e501832 100644 --- a/src/data.jl +++ b/src/data.jl @@ -172,6 +172,125 @@ function get_KVLCC2_L7_params() basic_params, maneuvering_params end +""" + wind_force_and_moment_coefficients(ψ_A,p) + +compute the parameter C_X,C_Y,C_N from Fujiwara's estimation method (1994). + +# Arguments +-`ψ_A`: Wind attack of angle [rad] +- L_pp +- B +- A_OD +- A_F +- A_L +- H_BR +- H_C +- C +""" +function wind_force_and_moment_coefficients( + ψ_A, + L_pp, + B, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, +) + #C_LF1の場合で調整 + C_CF = 0.404 + 0.368 * A_F / (B * H_BR) + 0.902 * H_BR / L_pp + + if deg2rad(0) <= ψ_A <= deg2rad(90) + C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp + C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) + C_ALF = -0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp + C_YLI = pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B) + + elseif deg2rad(90) < ψ_A <= deg2rad(180) + C_LF = + 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - + 0.341 * A_F / B^2 + C_XLI = + -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - + 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) + C_ALF = -0.314 - 1.117 * A_OD / A_L + C_YLI = pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2 + + elseif deg2rad(180) < ψ_A <= deg2rad(270) + C_LF = + 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - + 0.341 * A_F / B^2 + C_XLI = + -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - + 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) + C_ALF = -(-0.314 - 1.117 * A_OD / A_L) + C_YLI = -(pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2) + + elseif deg2rad(270) < ψ_A <= deg2rad(360) + C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp + C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) + C_ALF = -(-0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp) + C_YLI = -(pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B)) + end + + C_X = + C_LF * cos(ψ_A) + + C_XLI * (sin(ψ_A) - sin(ψ_A) * cos(ψ_A)^2 / 2) * sin(ψ_A) * cos(ψ_A) + + C_ALF * sin(ψ_A) * cos(ψ_A)^3 + C_Y = + C_CF * sin(ψ_A)^2 + + C_YLI * (cos(ψ_A) + sin(ψ_A)^2 * cos(ψ_A) / 2) * sin(ψ_A) * cos(ψ_A) + C_N = C_Y * (0.297 * C / L_pp - 0.149 * (ψ_A - deg2rad(90))) + + C_X, C_Y, C_N +end + +function get_example_ship_wind_force_moment_params() + L_pp = 7.00 # 船長Lpp[m] + B = 1.27 # 船幅[m] + d = 0.46 # 喫水[m] + D = 0.6563 # 深さ[m] + A_OD = 0.65 # デッキ上の構造物の側面投影面積[m^2] + H_BR = 0.85 # 喫水からブリッジ主要構造物の最高位[m] + H_C = 0.235 # 喫水から側面積中心までの高さ[m] + C = 0.0 # 船体中心から側面積中心までの前後方向座標(船首方向を正)[m] + + A_OD = A_OD # デッキ上の構造物の側面投影面積[m^2] + A_F = (D - d) * B # 船体の正面投影面積[m^2] + A_L = (D - d) * L_pp # 船体の側面投影面積[m^2] + H_BR = H_BR # 喫水からブリッジ主要構造物の最高位[m] + H_C = H_C # 喫水から側面積中心までの高さ[m] + C = C # 船体中心から側面積中心までの前後方向座標[m] + + ψ_A_vec = deg2rad.(collect(0:10:360)) + C_X_vec = Array{Float64}(undef, length(ψ_A_vec)) + C_Y_vec = Array{Float64}(undef, length(ψ_A_vec)) + C_N_vec = Array{Float64}(undef, length(ψ_A_vec)) + for (index, ψ_A) in enumerate(ψ_A_vec) + C_X, C_Y, C_N = wind_force_and_moment_coefficients( + ψ_A, + L_pp, + B, + A_OD, + A_F, + A_L, + H_BR, + H_C, + C, + ) + C_X_vec[index] = C_X + C_Y_vec[index] = C_Y + C_N_vec[index] = C_N + end + spl_C_X = Spline1D(ψ_A_vec, C_X_vec) + spl_C_Y = Spline1D(ψ_A_vec, C_Y_vec) + spl_C_N = Spline1D(ψ_A_vec, C_N_vec) + + spl_C_X, spl_C_Y, spl_C_N +end + function calc_position(time_vec, u_vec, v_vec, r_vec; x0=0.0, y0=0.0, ψ0=0.0) dim = length(time_vec) x_vec = zeros(Float64, dim) @@ -180,11 +299,13 @@ function calc_position(time_vec, u_vec, v_vec, r_vec; x0=0.0, y0=0.0, ψ0=0.0) x_vec[1] = x0 y_vec[1] = y0 ψ_vec[1] = ψ0 - for i = 2:dim + for i in 2:dim Δt = time_vec[i] - time_vec[i-1] ψ_vec[i] = ψ_vec[i-1] + r_vec[i] * Δt - x_vec[i] = x_vec[i-1] + (u_vec[i] * cos(ψ_vec[i]) - v_vec[i] * sin(ψ_vec[i])) * Δt - y_vec[i] = y_vec[i-1] + (u_vec[i] * sin(ψ_vec[i]) + v_vec[i] * cos(ψ_vec[i])) * Δt + x_vec[i] = + x_vec[i-1] + (u_vec[i] * cos(ψ_vec[i]) - v_vec[i] * sin(ψ_vec[i])) * Δt + y_vec[i] = + y_vec[i-1] + (u_vec[i] * sin(ψ_vec[i]) + v_vec[i] * cos(ψ_vec[i])) * Δt end x_vec, y_vec, ψ_vec end @@ -194,7 +315,7 @@ function nuts_sampling_single_thread( n_samples::Int, n_chains::Int; target_acceptance::Float64=0.65, - progress=true + progress=true, ) sampler = NUTS(target_acceptance) mapreduce( @@ -209,7 +330,7 @@ function nuts_sampling_multi_threads( n_samples::Int, n_chains::Int; target_acceptance::Float64=0.65, - progress=false + progress=false, ) sampler = NUTS(target_acceptance) sample(model, sampler, MCMCThreads(), n_samples, n_chains, progress=progress) diff --git a/src/mmg.jl b/src/mmg.jl index b9a28be..6ecf7e0 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -21,81 +21,6 @@ function get_wind_attack_of_angle( ψ_A end -""" - wind_force_and_moment_coefficients(ψ,ψ_wind,p) - -conpute the parameter C_X,C_Y,C_N - -# Arguments --`ψ_A`: Wind attack of angle [rad] -- L_pp -- B -- A_OD -- A_F -- A_L -- H_BR -- H_C -- C -""" -function wind_force_and_moment_coefficients( - ψ_A, - L_pp, - B, - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, -) - #C_LF1の場合で調整 - C_CF = 0.404 + 0.368 * A_F / (B * H_BR) + 0.902 * H_BR / L_pp - - if deg2rad(0) <= ψ_A <= deg2rad(90) - C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp - C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) - C_ALF = -0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp - C_YLI = pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B) - - elseif deg2rad(90) < ψ_A <= deg2rad(180) - C_LF = - 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - - 0.341 * A_F / B^2 - C_XLI = - -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) - C_ALF = -0.314 - 1.117 * A_OD / A_L - C_YLI = pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2 - - elseif deg2rad(180) < ψ_A <= deg2rad(270) - C_LF = - 0.018 - 5.091 * B / L_pp + 10.367 * H_C / L_pp - 3.011 * A_OD / L_pp^2 - - 0.341 * A_F / B^2 - C_XLI = - -1.901 + 12.727 * A_L / (L_pp * H_BR) + 24.407 * A_F / A_L - - 40.310 * B / L_pp - 0.341 * A_F / (B * H_BR) - C_ALF = -(-0.314 - 1.117 * A_OD / A_L) - C_YLI = -(pi * A_L / L_pp^2 + 0.446 + 2.192 * A_F / L_pp^2) - - elseif deg2rad(270) < ψ_A <= deg2rad(360) - C_LF = -0.992 + 0.507 * A_L / (L_pp * B) + 1.162 * C / L_pp - C_XLI = 0.458 + 3.245 * A_L / (L_pp * H_BR) - 2.313 * A_F / (B * H_BR) - C_ALF = -(-0.585 - 0.906 * A_OD / A_L + 3.239 * B / L_pp) - C_YLI = -(pi * A_L / L_pp^2 + 0.116 + 3.345 * A_F / (L_pp * B)) - end - - C_X = - C_LF * cos(ψ_A) + - C_XLI * (sin(ψ_A) - sin(ψ_A) * cos(ψ_A)^2 / 2) * sin(ψ_A) * cos(ψ_A) + - C_ALF * sin(ψ_A) * cos(ψ_A)^3 - C_Y = - C_CF * sin(ψ_A)^2 + - C_YLI * (cos(ψ_A) + sin(ψ_A)^2 * cos(ψ_A) / 2) * sin(ψ_A) * cos(ψ_A) - C_N = C_Y * (0.297 * C / L_pp - 0.149 * (ψ_A - deg2rad(90))) - - C_X, C_Y, C_N -end - """ mmg_3dof_model!(dX, X, p, t) From 4dd70f6eeb4d131b7bfa9c21c208542fe44789d0 Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 16:54:30 +0900 Subject: [PATCH 11/12] add Mmg3DofWindForceMomentParams struct --- src/ShipMMG.jl | 1 + src/data.jl | 2 +- src/mmg.jl | 53 ++++++++++++++++++++++++++++++++++-------------- test/test_mmg.jl | 4 ++++ 4 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/ShipMMG.jl b/src/ShipMMG.jl index adf90e3..aaed570 100644 --- a/src/ShipMMG.jl +++ b/src/ShipMMG.jl @@ -30,6 +30,7 @@ include("mmg.jl") export Mmg3DofBasicParams, Mmg3DofManeuveringParams, Mmg3DofStructureParams, + Mmg3DofWindForceMomentParams, mmg_3dof_simulate, mmg_3dof_model!, mmg_3dof_zigzag_test, diff --git a/src/data.jl b/src/data.jl index e501832..0e20a45 100644 --- a/src/data.jl +++ b/src/data.jl @@ -288,7 +288,7 @@ function get_example_ship_wind_force_moment_params() spl_C_Y = Spline1D(ψ_A_vec, C_Y_vec) spl_C_N = Spline1D(ψ_A_vec, C_N_vec) - spl_C_X, spl_C_Y, spl_C_N + Mmg3DofWindForceMomentParams(A_F, A_L, spl_C_X, spl_C_Y, spl_C_N) end function calc_position(time_vec, u_vec, v_vec, r_vec; x0=0.0, y0=0.0, ψ0=0.0) diff --git a/src/mmg.jl b/src/mmg.jl index 6ecf7e0..446a327 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -142,6 +142,9 @@ function mmg_3dof_model!(dX, X, p, t) N_vvr_dash, N_vrr_dash, N_rrr_dash, + spl_C_X, + spl_C_Y, + spl_C_N, spl_δ, spl_n_p, spl_u_wind, @@ -255,22 +258,10 @@ function mmg_3dof_model!(dX, X, p, t) N_R = -(x_R + a_H * x_H) * F_N * cos(δ) ψ_A = get_wind_attack_of_angle(ψ, ψ_wind) - C_X_wind, C_Y_wind, C_N_wind = wind_force_and_moment_coefficients( - ψ_A, - L_pp, - B, - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, - ) - ρ_air = 1.225 - X_wind = ρ_air * A_F * C_X_wind / 2 * u_wind^2 - Y_wind = ρ_air * A_L * C_Y_wind / 2 * u_wind^2 - N_wind = ρ_air * A_L * L_pp * C_N_wind / 2 * u_wind^2 + X_wind = ρ_air * A_F * spl_C_X(ψ_A) / 2 * u_wind^2 + Y_wind = ρ_air * A_L * spl_C_Y(ψ_A) / 2 * u_wind^2 + N_wind = ρ_air * A_L * L_pp * spl_C_N(ψ_A) / 2 * u_wind^2 dX[1] = du = @@ -419,6 +410,24 @@ Sturucture parameters of target ship for MMG 3DOF simulation. C::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_wind_list, ψ_wind_list basic_params, maneuvering_params,structure_params, [ u0, v0, r0, x0, y0, ψ0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, ψ, δ, n_p @@ -477,6 +486,7 @@ function mmg_3dof_simulate( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, structure_params::Mmg3DofStructureParams, + wind_force_and_moment_params::Mmg3DofWindForceMomentParams, time_list, δ_list, n_p_list, @@ -547,6 +557,8 @@ function mmg_3dof_simulate( H_C, C = structure_params + @unpack A_F, A_L, spl_C_X, spl_C_Y, spl_C_N = wind_force_and_moment_params + simulate( L_pp, B, @@ -599,6 +611,9 @@ function mmg_3dof_simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash, + spl_C_X, + spl_C_Y, + spl_C_N, time_list, δ_list, n_p_list, @@ -743,6 +758,9 @@ function simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash, + spl_C_X, + spl_C_Y, + spl_C_N, time_list, δ_list, n_p_list, @@ -820,6 +838,9 @@ function simulate( N_vvr_dash, N_vrr_dash, N_rrr_dash, + spl_C_X, + spl_C_Y, + spl_C_N, spl_δ, spl_n_p, spl_u_wind, @@ -901,6 +922,7 @@ function mmg_3dof_zigzag_test( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, structure_params::Mmg3DofStructureParams, + wind_force_and_moment_params::Mmg3DofWindForceMomentParams, time_list, n_p_list, target_δ_rad, @@ -976,6 +998,7 @@ function mmg_3dof_zigzag_test( basic_params, maneuvering_params, structure_params, + wind_force_and_moment_params, time_list[start_index:end], δ_list, n_p_list[start_index:end], diff --git a/test/test_mmg.jl b/test/test_mmg.jl index eba8082..8da9915 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -1,5 +1,6 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() structure_params = get_structure_params() +wind_force_and_moment_params = get_example_ship_wind_force_moment_params() @testset "mmg.jl KVLCC2_L7 turning" begin duration = 200 # [s] @@ -21,6 +22,7 @@ structure_params = get_structure_params() basic_params, maneuvering_params, structure_params, + wind_force_and_moment_params, time_list, δ_rad_list, n_p_list, @@ -53,6 +55,7 @@ end basic_params, maneuvering_params, structure_params, + wind_force_and_moment_params, time_list, n_p_list, target_δ_rad, @@ -81,6 +84,7 @@ end basic_params, maneuvering_params, structure_params, + wind_force_and_moment_params, time_list, δ_rad_list, n_p_list, From 5579ad150c79ded715a78803c41cad2843f52072 Mon Sep 17 00:00:00 2001 From: Taiga MITSUYUKI Date: Wed, 19 Jul 2023 19:54:22 +0900 Subject: [PATCH 12/12] remove Mmg3DofStructureParams struct --- src/ShipMMG.jl | 2 - src/data.jl | 28 -------------- src/mmg.jl | 99 +++++++++++++----------------------------------- test/test_mmg.jl | 4 -- 4 files changed, 27 insertions(+), 106 deletions(-) diff --git a/src/ShipMMG.jl b/src/ShipMMG.jl index aaed570..93f1aeb 100644 --- a/src/ShipMMG.jl +++ b/src/ShipMMG.jl @@ -12,7 +12,6 @@ export calc_position, ShipData, get_KVLCC2_L7_basic_params, get_KVLCC2_L7_maneuvering_params, - get_structure_params, get_KVLCC2_L7_params, get_example_ship_wind_force_moment_params, nuts_sampling_single_thread, @@ -29,7 +28,6 @@ export kt_simulate, include("mmg.jl") export Mmg3DofBasicParams, Mmg3DofManeuveringParams, - Mmg3DofStructureParams, Mmg3DofWindForceMomentParams, mmg_3dof_simulate, mmg_3dof_model!, diff --git a/src/data.jl b/src/data.jl index 0e20a45..a4279a8 100644 --- a/src/data.jl +++ b/src/data.jl @@ -138,34 +138,6 @@ function get_KVLCC2_L7_maneuvering_params() maneuvering_params end -function get_structure_params() - L_pp = 7.00 # 船長Lpp[m] - B = 1.27 # 船幅[m] - d = 0.46 # 喫水[m] - D = 0.6563 # 深さ[m] - A_OD = 0.65 # デッキ上の構造物の側面投影面積[m^2] - H_BR = 0.85 # 喫水からブリッジ主要構造物の最高位[m] - H_C = 0.235 # 喫水から側面積中心までの高さ[m] - C = 0.0 # 船体中心から側面積中心までの前後方向座標(船首方向を正)[m] - - A_OD = A_OD # デッキ上の構造物の側面投影面積[m^2] - A_F = (D - d) * B # 船体の正面投影面積[m^2] - A_L = (D - d) * L_pp # 船体の側面投影面積[m^2] - H_BR = H_BR # 喫水からブリッジ主要構造物の最高位[m] - H_C = H_C # 喫水から側面積中心までの高さ[m] - C = C # 船体中心から側面積中心までの前後方向座標[m] - - structure_params = Mmg3DofStructureParams( - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, - ) - structure_params -end - function get_KVLCC2_L7_params() basic_params = get_KVLCC2_L7_basic_params() maneuvering_params = get_KVLCC2_L7_maneuvering_params() diff --git a/src/mmg.jl b/src/mmg.jl index 446a327..9c6893c 100644 --- a/src/mmg.jl +++ b/src/mmg.jl @@ -1,4 +1,6 @@ """ +get_wind_attack_of_angle(ψ_ship,ψ_wind) + Wind angle of attack - 0 [rad] point out to North and rotating clockwise positive angle. """ function get_wind_attack_of_angle( @@ -56,12 +58,6 @@ MMG 3DOF model on DifferentialEquations.ODEProblem. Update `dX`. - t_P - w_P0 - x_P - - A_OD - - A_F - - A_L - - H_BR - - H_C - - C - k_0 - k_1 - k_2 @@ -82,6 +78,11 @@ 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 @@ -116,12 +117,6 @@ function mmg_3dof_model!(dX, X, p, t) t_P, w_P0, x_P, - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, k_0, k_1, k_2, @@ -142,6 +137,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, @@ -390,26 +387,6 @@ Maneuvering parameters of target ship for MMG 3DOF simulation. N_rrr_dash::T end -""" -Sturucture parameters of target ship for MMG 3DOF simulation. - -# Arguments -- `A_OD::T` -- `A_F::T` -- `A_L::T` -- `H_BR::T` -- `H_C::T` -- `C::T` -""" -@with_kw mutable struct Mmg3DofStructureParams{T} - A_OD::T - A_F::T - A_L::T - H_BR::T - H_C::T - C::T -end - """ Wind force and moment parameters of target ship for MMG 3DOF simulation. @@ -429,7 +406,7 @@ Wind force and moment parameters of target ship for MMG 3DOF simulation. end """ - mmg_3dof_simulate(time_list, n_p_list, δ_list, u_wind_list, ψ_wind_list basic_params, maneuvering_params,structure_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, u_wind_list, ψ_wind_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 simulation results including the lists of time, u, v, r, x, y, ψ, δ, n_p. This function has the same logic of `ShipMMG.simulate()`. @@ -437,7 +414,7 @@ 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. -- `structure_params::Mmg3DofStructureParams,` : the Structur parameters above the taeget ship's draft +- `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. @@ -485,7 +462,6 @@ julia> mmg_results = mmg_3dof_simulate( function mmg_3dof_simulate( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, - structure_params::Mmg3DofStructureParams, wind_force_and_moment_params::Mmg3DofWindForceMomentParams, time_list, δ_list, @@ -550,13 +526,6 @@ function mmg_3dof_simulate( N_vrr_dash, N_rrr_dash = maneuvering_params - @unpack A_OD, - A_F, - A_L, - H_BR, - H_C, - C = structure_params - @unpack A_F, A_L, spl_C_X, spl_C_Y, spl_C_N = wind_force_and_moment_params simulate( @@ -585,12 +554,6 @@ function mmg_3dof_simulate( t_P, w_P0, x_P, - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, k_0, k_1, k_2, @@ -611,6 +574,8 @@ 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, @@ -633,7 +598,7 @@ function mmg_3dof_simulate( end """ - simulate(time_list, n_p_list, δ_list, u_wind_list, ψ_wind_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, [, u0, v0, r0, ρ, algorithm, reltol, abstol]) -> u, v, r, x, y, ψ, δ, n_p + simulate(time_list, n_p_list, δ_list, u_wind_list, ψ_wind_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 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()`. @@ -664,12 +629,6 @@ This function has the same logic of `ShipMMG.mmg_3dof_simulate()`. - `t_P`: - `w_P0`: - `x_P`: -- `A_OD:` -- `A_F:` -- `A_L:` -- `H_BR:` -- `H_C:` -- `C:` - `k_0` - `k_1` - `k_2` @@ -690,6 +649,11 @@ 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. @@ -732,12 +696,6 @@ function simulate( t_P, w_P0, x_P, - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, k_0, k_1, k_2, @@ -758,6 +716,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, @@ -812,12 +772,6 @@ function simulate( t_P, w_P0, x_P, - A_OD, - A_F, - A_L, - H_BR, - H_C, - C, k_0, k_1, k_2, @@ -838,6 +792,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, @@ -864,14 +820,14 @@ function simulate( end """ - mmg_3dof_zigzag_test(basic_params, maneuvering_params,structure_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,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, ψ, δ 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. -- `structure_params::Mmg3DofStructureParams,` : the Structur parameters above the taeget ship's draft +- `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. @@ -909,7 +865,8 @@ julia> ψ_wind_list = n_const .* ones(Float64, sampling); julia> δ_list, u_list, v_list, r_list, ψ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, - time_list + wind_force_and_moment_params, + time_list, n_p_list, u_wind_list, ψ_wind_list, @@ -921,7 +878,6 @@ julia> δ_list, u_list, v_list, r_list, ψ_list = mmg_3dof_zigzag_test( function mmg_3dof_zigzag_test( basic_params::Mmg3DofBasicParams, maneuvering_params::Mmg3DofManeuveringParams, - structure_params::Mmg3DofStructureParams, wind_force_and_moment_params::Mmg3DofWindForceMomentParams, time_list, n_p_list, @@ -997,7 +953,6 @@ function mmg_3dof_zigzag_test( u, v, r, x, y, ψ_list, δ, n_p = mmg_3dof_simulate( basic_params, maneuvering_params, - structure_params, wind_force_and_moment_params, time_list[start_index:end], δ_list, diff --git a/test/test_mmg.jl b/test/test_mmg.jl index 8da9915..fe2fa9c 100644 --- a/test/test_mmg.jl +++ b/test/test_mmg.jl @@ -1,5 +1,4 @@ basic_params, maneuvering_params = get_KVLCC2_L7_params() -structure_params = get_structure_params() wind_force_and_moment_params = get_example_ship_wind_force_moment_params() @testset "mmg.jl KVLCC2_L7 turning" begin @@ -21,7 +20,6 @@ wind_force_and_moment_params = get_example_ship_wind_force_moment_params() mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, - structure_params, wind_force_and_moment_params, time_list, δ_rad_list, @@ -54,7 +52,6 @@ end u_list, v_list, r_list, x_list, y_list, ψ_list, δ_list = mmg_3dof_zigzag_test( basic_params, maneuvering_params, - structure_params, wind_force_and_moment_params, time_list, n_p_list, @@ -83,7 +80,6 @@ end mmg_results = mmg_3dof_simulate( basic_params, maneuvering_params, - structure_params, wind_force_and_moment_params, time_list, δ_rad_list,