Added more noise simulation and a multi-simulate file that auto-runs multiple simulations with a desired noise level.
This commit is contained in:
@@ -6,44 +6,136 @@ Ported from quadParamsScript.m and constantsScript.m
|
||||
import numpy as np
|
||||
|
||||
|
||||
# Global parameter variations - initialized once per simulation run
|
||||
_param_variations = None
|
||||
|
||||
|
||||
def initialize_parameter_variations(noise_level=0.05, seed=None):
|
||||
"""
|
||||
Initialize mechanical and electrical parameter variations.
|
||||
Call this once at the start of a simulation to set random variations.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
noise_level : float, optional
|
||||
Standard deviation of multiplicative noise (default: 0.05 = 5%)
|
||||
seed : int, optional
|
||||
Random seed for reproducibility
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict
|
||||
Dictionary with parameter variation factors
|
||||
"""
|
||||
global _param_variations
|
||||
|
||||
if seed is not None:
|
||||
np.random.seed(seed)
|
||||
|
||||
# Generate multiplicative variation factors for each parameter
|
||||
_param_variations = {
|
||||
'mass': 1 + np.random.normal(0, noise_level),
|
||||
'Jq_components': np.array([1 + np.random.normal(0, noise_level) for _ in range(3)]), # Individual noise for each inertia component
|
||||
'frame_l': 1 + np.random.normal(0, noise_level * 0.5), # Smaller variation for dimensions
|
||||
'frame_w': 1 + np.random.normal(0, noise_level * 0.5),
|
||||
'yh': 1 + np.random.normal(0, noise_level * 0.5),
|
||||
# Individual yoke variations (4 yokes)
|
||||
'yoke_R': np.array([1 + np.random.normal(0, noise_level * 0.3) for _ in range(4)]),
|
||||
'yoke_L': np.array([1 + np.random.normal(0, noise_level * 0.3) for _ in range(4)]),
|
||||
# Position noise for yoke/rotor locations (4 locations, 3D)
|
||||
'rotor_pos_noise': np.random.normal(0, noise_level * 0.2, (3, 4)),
|
||||
# Position noise for sensor locations (4 sensors, 3D)
|
||||
'sensor_pos_noise': np.random.normal(0, noise_level * 0.2, (3, 4))
|
||||
}
|
||||
|
||||
return _param_variations.copy()
|
||||
|
||||
|
||||
def get_parameter_variations():
|
||||
"""
|
||||
Get current parameter variations. If not initialized, use nominal (no variation).
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict
|
||||
Dictionary with parameter variation factors
|
||||
"""
|
||||
global _param_variations
|
||||
|
||||
if _param_variations is None:
|
||||
# Use nominal values (no variation)
|
||||
_param_variations = {
|
||||
'mass': 1.0,
|
||||
'Jq_components': np.ones(3),
|
||||
'frame_l': 1.0,
|
||||
'frame_w': 1.0,
|
||||
'yh': 1.0,
|
||||
'yoke_R': np.ones(4),
|
||||
'yoke_L': np.ones(4),
|
||||
'rotor_pos_noise': np.zeros((3, 4)),
|
||||
'sensor_pos_noise': np.zeros((3, 4))
|
||||
}
|
||||
|
||||
return _param_variations
|
||||
|
||||
|
||||
def reset_parameter_variations():
|
||||
"""Reset parameter variations to force reinitialization"""
|
||||
global _param_variations
|
||||
_param_variations = None
|
||||
|
||||
|
||||
class QuadParams:
|
||||
"""Quadrotor/maglev pod parameters"""
|
||||
|
||||
def __init__(self):
|
||||
# Pod mechanical characteristics
|
||||
frame_l = 0.61
|
||||
frame_w = 0.149 # in meters
|
||||
self.yh = 3 * 0.0254 # yoke height
|
||||
# Get parameter variations
|
||||
pv = get_parameter_variations()
|
||||
|
||||
# Pod mechanical characteristics (with variations)
|
||||
frame_l = 0.61 * pv['frame_l']
|
||||
frame_w = 0.149 * pv['frame_w']
|
||||
self.yh = 3 * 0.0254 * pv['yh'] # yoke height
|
||||
yh = self.yh
|
||||
|
||||
# Yoke/rotor locations (at corners)
|
||||
self.rotor_loc = np.array([
|
||||
# Store dimensions for reference
|
||||
self.frame_l = frame_l
|
||||
self.frame_w = frame_w
|
||||
|
||||
# Yoke/rotor locations (at corners) with position noise
|
||||
nominal_rotor_loc = np.array([
|
||||
[frame_l/2, frame_l/2, -frame_l/2, -frame_l/2],
|
||||
[-frame_w/2, frame_w/2, frame_w/2, -frame_w/2],
|
||||
[yh, yh, yh, yh]
|
||||
])
|
||||
self.rotor_loc = nominal_rotor_loc * (1 + pv['rotor_pos_noise'])
|
||||
|
||||
# Sensor locations (independent from yoke/rotor locations, at edge centers)
|
||||
self.sensor_loc = np.array([
|
||||
# Sensor locations (independent from yoke/rotor locations, at edge centers) with position noise
|
||||
nominal_sensor_loc = np.array([
|
||||
[frame_l/2, 0, -frame_l/2, 0],
|
||||
[0, frame_w/2, 0, -frame_w/2],
|
||||
[yh, yh, yh, yh]
|
||||
])
|
||||
self.sensor_loc = nominal_sensor_loc * (1 + pv['sensor_pos_noise'])
|
||||
|
||||
self.gap_sigma = 0.5e-3 # usually on micron scale
|
||||
|
||||
# Mass of the quad, in kg
|
||||
self.m = 6
|
||||
# Mass of the quad, in kg (with variation)
|
||||
self.m = 6 * pv['mass']
|
||||
|
||||
# The quad's moment of inertia, expressed in the body frame, in kg-m^2
|
||||
self.Jq = np.diag([0.017086, 0.125965, 0.131940])
|
||||
# The quad's moment of inertia, expressed in the body frame, in kg-m^2 (with individual component variations)
|
||||
nominal_Jq = np.diag([0.017086, 0.125965, 0.131940])
|
||||
self.Jq = nominal_Jq * np.diag(pv['Jq_components']) # Apply different noise to each diagonal component
|
||||
self.invJq = np.linalg.inv(self.Jq)
|
||||
|
||||
# Quad electrical characteristics
|
||||
# Quad electrical characteristics (with variations)
|
||||
maxcurrent = 30
|
||||
self.yokeR = 2.2 # in ohms
|
||||
self.yokeL = 5e-3 # in henries (2.5mH per yoke)
|
||||
self.maxVoltage = maxcurrent * self.yokeR # max magnitude voltage supplied to each yoke
|
||||
|
||||
# Individual yoke resistances and inductances (4 yokes with individual variations)
|
||||
self.yokeR_individual = 2.2 * pv['yoke_R'] # 4-element array
|
||||
self.yokeL_individual = 5e-3 * pv['yoke_L'] # 4-element array
|
||||
|
||||
self.maxVoltage = maxcurrent * self.yokeR_individual # max magnitude voltage supplied to each yoke
|
||||
|
||||
|
||||
class Constants:
|
||||
|
||||
Reference in New Issue
Block a user