迁移RTK-IMU标定到独立顶层包
This commit is contained in:
lichun.qu
@@ -0,0 +1,529 @@
|
||||
'''Per-GNSS-node RTK/IMU state graph used after legacy propagation deprecation.'''
|
||||
from __future__ import annotations
|
||||
from dataclasses import dataclass
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.sparse import lil_matrix
|
||||
from imu_lidar.geometry import so3_exp, so3_log
|
||||
from imu_lidar.imu_preintegration import apply_bias_correction_imu, residual_whiten_matrix
|
||||
from .rtk_imu_engineering import (
|
||||
G_ENU, HPR_DIRECT_ANGULAR_SIGMA_RAD, _Segment, _world_rtk)
|
||||
|
||||
NODE_DOF = 15
|
||||
SIGMA_BG_RW = 1e-5
|
||||
SIGMA_BA_RW = 1e-3
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NodeGraphProblem:
|
||||
segment: _Segment
|
||||
R_seed_WI: tuple[np.ndarray,...]
|
||||
fixed_l_I_m: np.ndarray
|
||||
R_RTK_IMU: np.ndarray
|
||||
hpr_direct_angular_sigma_rad: float = HPR_DIRECT_ANGULAR_SIGMA_RAD
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NodeGraphResult:
|
||||
success: bool
|
||||
message: str
|
||||
node_count: int
|
||||
duration_s: float
|
||||
fixed_l_I_m: np.ndarray
|
||||
initial_cost: float
|
||||
final_cost: float
|
||||
cost_reduction: float
|
||||
nfev: int
|
||||
optimality: float
|
||||
gradient_norm: float
|
||||
residual_dimension: int
|
||||
state_dimension: int
|
||||
statistical_dof: int
|
||||
total_nis: float
|
||||
chi_square_per_dof: float
|
||||
cost_per_dof: float
|
||||
initial_residual_by_factor: dict[str,dict[str,float]]
|
||||
final_residual_by_factor: dict[str,dict[str,float]]
|
||||
final_position_residual_m: dict[str,object]
|
||||
final_velocity_residual_m_s: dict[str,object]
|
||||
max_bg_step_rad_s: float
|
||||
max_ba_step_m_s2: float
|
||||
preintegration_covariance_sigma: dict[str,dict[str,float]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FreeLeverResult:
|
||||
success: bool
|
||||
message: str
|
||||
initial_l_I_m: np.ndarray
|
||||
final_l_I_m: np.ndarray
|
||||
lever_step_norm_m: float
|
||||
initial_cost: float
|
||||
final_cost: float
|
||||
nfev: int
|
||||
optimality: float
|
||||
chi_square_per_dof: float
|
||||
position_residual_m: dict[str,object]
|
||||
velocity_residual_m_s: dict[str,object]
|
||||
residual_by_factor: dict[str,dict[str,float]]
|
||||
lever_covariance_m2: np.ndarray
|
||||
lever_information_singular_values: np.ndarray
|
||||
lever_information_condition_number: float
|
||||
lever_precision_rank: int
|
||||
weakest_lever_direction_I: np.ndarray
|
||||
|
||||
def _rotation(problem,index,x):
|
||||
offset = NODE_DOF*index
|
||||
return problem.R_seed_WI[index] @ so3_exp(x[offset:offset+3])
|
||||
|
||||
def initial_parameters(problem, lever_override=None):
|
||||
nodes = problem.segment.nodes
|
||||
x = np.zeros(NODE_DOF*len(nodes))
|
||||
lever = (problem.fixed_l_I_m if lever_override is None
|
||||
else np.asarray(lever_override,dtype=float))
|
||||
previous_p, previous_v = np.zeros(3), np.zeros(3)
|
||||
for index,node in enumerate(nodes):
|
||||
offset = NODE_DOF*index
|
||||
R = problem.R_seed_WI[index]
|
||||
previous_p = node.p_enu_m-R@lever
|
||||
if index and not node.position_mask[2]:
|
||||
previous_p[2] = x[offset-NODE_DOF+5]
|
||||
if node.velocity_enu_m_s is not None:
|
||||
previous_v = node.velocity_enu_m_s-R@np.cross(node.gyro_rad_s,lever)
|
||||
x[offset+3:offset+6] = previous_p
|
||||
x[offset+6:offset+9] = previous_v
|
||||
return x
|
||||
|
||||
def _stats(values, effective_dof=None):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
|
||||
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
|
||||
'chi_square_per_dof':np.nan}
|
||||
nis = float(np.dot(a,a))
|
||||
dof = int(a.size if effective_dof is None else effective_dof)
|
||||
return {'count':int(a.size),'dof':dof,
|
||||
'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50.)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99.)),
|
||||
'nis':nis,'chi_square_per_dof':nis/max(dof,1)}
|
||||
|
||||
def _hpr_sigma(problem, node):
|
||||
old = float(node.hpr_angular_sigma_rad)
|
||||
extra_var = max(old*old-HPR_DIRECT_ANGULAR_SIGMA_RAD**2, 0.)
|
||||
return float(np.sqrt(problem.hpr_direct_angular_sigma_rad**2+extra_var))
|
||||
|
||||
|
||||
def _distribution(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p50':np.nan,'p95':np.nan,'p99':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50':float(np.percentile(a,50.)),'p95':float(np.percentile(a,95.)),
|
||||
'p99':float(np.percentile(a,99.))}
|
||||
|
||||
def _vector_stats(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not a.size:
|
||||
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
|
||||
'vector_rms':np.nan,'vector_p95':np.nan}
|
||||
norm = np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95.,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95.))}
|
||||
|
||||
def residual(problem,x,details=None,dependencies=None,lever_override=None):
|
||||
nodes, preints = problem.segment.nodes, problem.segment.preintegrations
|
||||
lever = (problem.fixed_l_I_m if lever_override is None
|
||||
else np.asarray(lever_override,dtype=float))
|
||||
baseline_I = problem.R_RTK_IMU.T[:,0]
|
||||
values = []
|
||||
def add(value,label,node_indices,raw=None):
|
||||
a = np.asarray(value,dtype=float).reshape(-1)
|
||||
values.extend(a)
|
||||
if dependencies is not None:
|
||||
dependencies.extend([tuple(node_indices)]*len(a))
|
||||
if details is not None:
|
||||
details.setdefault(label,[]).extend(a.tolist())
|
||||
if raw is not None: details.setdefault(label+'_physical',[]).append(np.asarray(raw))
|
||||
for index,node in enumerate(nodes):
|
||||
offset = NODE_DOF*index
|
||||
R = _rotation(problem,index,x)
|
||||
p, v = x[offset+3:offset+6], x[offset+6:offset+9]
|
||||
bg, ba = x[offset+9:offset+12], x[offset+12:offset+15]
|
||||
p_error = p+R@lever-node.p_enu_m
|
||||
if node.source == 'GGA':
|
||||
add(p_error[:2]/.06,'gga_xy',(index,))
|
||||
if details is not None: details.setdefault('position_physical',[]).append(
|
||||
np.array([p_error[0],p_error[1],np.nan]))
|
||||
else:
|
||||
add(p_error/np.array([.06,.06,.12]),'best_position',(index,),p_error)
|
||||
if details is not None: details.setdefault('position_physical',[]).append(p_error)
|
||||
if node.velocity_enu_m_s is not None:
|
||||
v_error = v+R@np.cross(node.gyro_rad_s-bg,lever)-node.velocity_enu_m_s
|
||||
add(v_error/np.array([.15,.15,.30]),'doppler',(index,),v_error)
|
||||
if details is not None: details.setdefault('velocity_physical',[]).append(v_error)
|
||||
if node.hpr_factor_valid:
|
||||
hpr_error=np.cross(R@baseline_I,node.baseline_enu)
|
||||
add(hpr_error/_hpr_sigma(problem,node),'hpr',(index,),hpr_error)
|
||||
if node.gravity_candidate:
|
||||
gravity = node.accel_m_s2-ba-R.T@(-G_ENU)
|
||||
add(gravity/.12,'gravity',(index,))
|
||||
if index == len(nodes)-1: continue
|
||||
right = index+1
|
||||
right_offset = NODE_DOF*right
|
||||
Rj = _rotation(problem,right,x)
|
||||
pj = x[right_offset+3:right_offset+6]
|
||||
vj = x[right_offset+6:right_offset+9]
|
||||
bgj = x[right_offset+9:right_offset+12]
|
||||
baj = x[right_offset+12:right_offset+15]
|
||||
pre = preints[index]
|
||||
dR,dv,dp = apply_bias_correction_imu(pre,bg,ba)
|
||||
dt = pre.duration_s
|
||||
imu_error = np.concatenate([
|
||||
so3_log(dR.T@R.T@Rj),
|
||||
R.T@(vj-v-G_ENU*dt)-dv,
|
||||
R.T@(pj-p-v*dt-.5*G_ENU*dt*dt)-dp])
|
||||
add(residual_whiten_matrix(pre.cov)@imu_error,'imu_preintegration',
|
||||
(index,right),imu_error)
|
||||
add((bgj-bg)/(SIGMA_BG_RW*np.sqrt(dt)),'gyro_bias_random_walk',(index,right))
|
||||
add((baj-ba)/(SIGMA_BA_RW*np.sqrt(dt)),'accel_bias_random_walk',(index,right))
|
||||
add(x[:3]/np.deg2rad(5.),'initial_attitude_gauge',(0,))
|
||||
add(x[9:12]/.02,'initial_gyro_bias',(0,))
|
||||
add(x[12:15]/.5,'initial_accel_bias',(0,))
|
||||
return np.asarray(values)
|
||||
|
||||
def jacobian_sparsity(problem,x):
|
||||
dependencies = []
|
||||
base = residual(problem,x,dependencies=dependencies)
|
||||
sparsity = lil_matrix((len(base),len(x)),dtype=int)
|
||||
for row,node_indices in enumerate(dependencies):
|
||||
for index in node_indices:
|
||||
start = NODE_DOF*index
|
||||
sparsity[row,start:start+NODE_DOF] = 1
|
||||
return sparsity.tocsr()
|
||||
|
||||
def _factor_stats(details):
|
||||
return {key:_stats(value,2*len(value)//3 if key=='hpr' else None)
|
||||
for key,value in details.items()
|
||||
if not key.endswith('_physical') and key not in ('position_physical','velocity_physical')}
|
||||
|
||||
|
||||
def _effective_residual_dimension(details):
|
||||
return sum(2*len(v)//3 if k=='hpr' else len(v)
|
||||
for k,v in details.items() if not k.endswith('_physical')
|
||||
and k not in ('position_physical','velocity_physical'))
|
||||
|
||||
def _preintegration_covariance_stats(problem):
|
||||
blocks = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
|
||||
for pre in problem.segment.preintegrations:
|
||||
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
|
||||
blocks['rotation_rad'].extend(sigma[:3])
|
||||
blocks['velocity_m_s'].extend(sigma[3:6])
|
||||
blocks['position_m'].extend(sigma[6:9])
|
||||
return {key:_distribution(value) for key,value in blocks.items()}
|
||||
|
||||
def build_problem(segment,R_RTK_IMU,fixed_l_I_m,
|
||||
hpr_direct_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD):
|
||||
seeds = []
|
||||
for index,node in enumerate(segment.nodes):
|
||||
if node.hpr_factor_valid:
|
||||
seeds.append(_world_rtk(node.baseline_enu)@R_RTK_IMU)
|
||||
elif index:
|
||||
seeds.append(seeds[-1]@segment.preintegrations[index-1].delta_R)
|
||||
else:
|
||||
seeds.append(segment.R_WRTK_initial@R_RTK_IMU)
|
||||
return NodeGraphProblem(segment,tuple(seeds),np.asarray(fixed_l_I_m,dtype=float),
|
||||
np.asarray(R_RTK_IMU,dtype=float),
|
||||
float(hpr_direct_angular_sigma_rad))
|
||||
|
||||
def solve_fixed_lever(problem,max_nfev=30):
|
||||
x0 = initial_parameters(problem)
|
||||
initial_detail = {}
|
||||
r0 = residual(problem,x0,initial_detail)
|
||||
fit = least_squares(
|
||||
lambda value:residual(problem,value),x0,jac='2-point',
|
||||
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',
|
||||
tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
final_detail = {}
|
||||
rf = residual(problem,fit.x,final_detail)
|
||||
statistical_dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
|
||||
bg = fit.x.reshape(-1,NODE_DOF)[:,9:12]
|
||||
ba = fit.x.reshape(-1,NODE_DOF)[:,12:15]
|
||||
bg_step = np.diff(bg,axis=0)
|
||||
ba_step = np.diff(ba,axis=0)
|
||||
return NodeGraphResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
node_count=len(problem.segment.nodes),
|
||||
duration_s=problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s,
|
||||
fixed_l_I_m=problem.fixed_l_I_m.copy(),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
gradient_norm=float(np.linalg.norm(fit.grad)),
|
||||
residual_dimension=len(rf),state_dimension=len(fit.x),
|
||||
statistical_dof=statistical_dof,total_nis=float(np.dot(rf,rf)),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/statistical_dof),
|
||||
cost_per_dof=.5*float(np.dot(rf,rf)/statistical_dof),
|
||||
initial_residual_by_factor=_factor_stats(initial_detail),
|
||||
final_residual_by_factor=_factor_stats(final_detail),
|
||||
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
|
||||
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
|
||||
max_bg_step_rad_s=float(np.max(np.linalg.norm(bg_step,axis=1))) if len(bg_step) else 0.,
|
||||
max_ba_step_m_s2=float(np.max(np.linalg.norm(ba_step,axis=1))) if len(ba_step) else 0.,
|
||||
preintegration_covariance_sigma=_preintegration_covariance_stats(problem))
|
||||
|
||||
|
||||
def fit_states_at_fixed_lever(problem,l_I_m,max_nfev=50,initial_state_values=None):
|
||||
lever=np.asarray(l_I_m,dtype=float)
|
||||
x0=(initial_parameters(problem,lever) if initial_state_values is None
|
||||
else np.asarray(initial_state_values,dtype=float))
|
||||
r0=residual(problem,x0,lever_override=lever)
|
||||
fit=least_squares(
|
||||
lambda value:residual(problem,value,lever_override=lever),x0,jac='2-point',
|
||||
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=max_nfev,x_scale='jac',
|
||||
ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
return fit.x,{'success':bool(fit.success),'message':str(fit.message),
|
||||
'nfev':int(fit.nfev),'initial_cost':.5*float(r0@r0),
|
||||
'cost':float(fit.cost)}
|
||||
|
||||
|
||||
def summarize_fixed_state_values(problems,state_values,l_I_m):
|
||||
details={}; residuals=[]
|
||||
for problem,value in zip(problems,state_values):
|
||||
local={}
|
||||
residuals.append(residual(problem,np.asarray(value),details=local,
|
||||
lever_override=l_I_m))
|
||||
for key,items in local.items(): details.setdefault(key,[]).extend(items)
|
||||
joined=np.concatenate(residuals)
|
||||
state_dimension=sum(len(value) for value in state_values)
|
||||
dof=max(_effective_residual_dimension(details)-state_dimension,1)
|
||||
return {'cost':.5*float(np.dot(joined,joined)),
|
||||
'total_nis':float(np.dot(joined,joined)),
|
||||
'chi_square_per_dof':float(np.dot(joined,joined)/dof),
|
||||
'statistical_dof':dof,'residual_by_factor':_factor_stats(details),
|
||||
'best_position_physical_m':_vector_stats(details.get('best_position_physical',[])),
|
||||
'doppler_physical_m_s':_vector_stats(details.get('doppler_physical',[])),
|
||||
'hpr_physical_rad':_vector_stats(details.get('hpr_physical',[]))}
|
||||
|
||||
def solve_fixed_lever_many(problems,max_nfev=30):
|
||||
problems = tuple(problems)
|
||||
sizes = [NODE_DOF*len(problem.segment.nodes) for problem in problems]
|
||||
offsets = np.cumsum([0,*sizes])
|
||||
x0 = np.concatenate([initial_parameters(problem) for problem in problems])
|
||||
def evaluate(value,details=None):
|
||||
chunks = []
|
||||
for index,problem in enumerate(problems):
|
||||
local_details = {} if details is not None else None
|
||||
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
|
||||
local_details))
|
||||
if details is not None:
|
||||
for key,items in local_details.items():
|
||||
details.setdefault(key,[]).extend(items)
|
||||
return np.concatenate(chunks)
|
||||
initial_detail = {}
|
||||
r0 = evaluate(x0,initial_detail)
|
||||
sparsity = lil_matrix((len(r0),len(x0)),dtype=int)
|
||||
row = 0
|
||||
for index,problem in enumerate(problems):
|
||||
local_x = x0[offsets[index]:offsets[index+1]]
|
||||
local = jacobian_sparsity(problem,local_x)
|
||||
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]] = local
|
||||
row += local.shape[0]
|
||||
fit = least_squares(
|
||||
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
|
||||
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
final_detail = {}
|
||||
rf = evaluate(fit.x,final_detail)
|
||||
bg_steps, ba_steps = [], []
|
||||
for index,problem in enumerate(problems):
|
||||
states = fit.x[offsets[index]:offsets[index+1]].reshape(-1,NODE_DOF)
|
||||
bg_steps.extend(np.linalg.norm(np.diff(states[:,9:12],axis=0),axis=1))
|
||||
ba_steps.extend(np.linalg.norm(np.diff(states[:,12:15],axis=0),axis=1))
|
||||
covariance = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
|
||||
for problem in problems:
|
||||
for pre in problem.segment.preintegrations:
|
||||
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
|
||||
covariance['rotation_rad'].extend(sigma[:3])
|
||||
covariance['velocity_m_s'].extend(sigma[3:6])
|
||||
covariance['position_m'].extend(sigma[6:9])
|
||||
dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
|
||||
return NodeGraphResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
node_count=sum(len(problem.segment.nodes) for problem in problems),
|
||||
duration_s=sum(problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s
|
||||
for problem in problems),
|
||||
fixed_l_I_m=problems[0].fixed_l_I_m.copy(),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
gradient_norm=float(np.linalg.norm(fit.grad)),
|
||||
residual_dimension=len(rf),state_dimension=len(fit.x),
|
||||
statistical_dof=dof,total_nis=float(np.dot(rf,rf)),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
cost_per_dof=.5*float(np.dot(rf,rf)/dof),
|
||||
initial_residual_by_factor=_factor_stats(initial_detail),
|
||||
final_residual_by_factor=_factor_stats(final_detail),
|
||||
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
|
||||
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
|
||||
max_bg_step_rad_s=float(max(bg_steps,default=0.)),
|
||||
max_ba_step_m_s2=float(max(ba_steps,default=0.)),
|
||||
preintegration_covariance_sigma={key:_distribution(value) for key,value in covariance.items()})
|
||||
|
||||
|
||||
def _free_residual(problem,value,details=None):
|
||||
return residual(problem,value[3:],details=details,lever_override=value[:3])
|
||||
|
||||
|
||||
def _free_sparsity(problem,value):
|
||||
local = jacobian_sparsity(problem,value[3:])
|
||||
result = lil_matrix((local.shape[0],local.shape[1]+3),dtype=int)
|
||||
result[:,:3] = 1
|
||||
result[:,3:] = local
|
||||
return result.tocsr()
|
||||
|
||||
|
||||
def _marginal_lever_information(jacobian):
|
||||
J = jacobian.toarray() if hasattr(jacobian,'toarray') else np.asarray(jacobian)
|
||||
H = J.T@J
|
||||
Hll,Hln,Hnn = H[:3,:3],H[:3,3:],H[3:,3:]
|
||||
marginal = Hll-Hln@np.linalg.pinv(Hnn,rcond=1e-10)@Hln.T
|
||||
return .5*(marginal+marginal.T)
|
||||
|
||||
|
||||
def _additive_marginal_lever_information(jacobian,row_offsets,state_offsets):
|
||||
total=np.zeros((3,3))
|
||||
for index in range(len(row_offsets)-1):
|
||||
rows=slice(row_offsets[index],row_offsets[index+1])
|
||||
columns=np.r_[0:3,state_offsets[index]:state_offsets[index+1]]
|
||||
local=jacobian[rows,:][:,columns]
|
||||
total+=_marginal_lever_information(local)
|
||||
return .5*(total+total.T)
|
||||
|
||||
|
||||
def linearized_lever_information(problem,l_I_m):
|
||||
lever=np.asarray(l_I_m,dtype=float)
|
||||
value=np.concatenate([lever,initial_parameters(problem,lever)])
|
||||
fit=least_squares(lambda x:_free_residual(problem,x),value,jac='2-point',
|
||||
jac_sparsity=_free_sparsity(problem,value),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=1,x_scale='jac')
|
||||
information=_marginal_lever_information(fit.jac)
|
||||
_,singular,Vt=np.linalg.svd(information)
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
return information,covariance,singular,Vt[-1]
|
||||
|
||||
|
||||
def solve_free_lever(problem,initial_l_I_m,max_nfev=120):
|
||||
initial_l = np.asarray(initial_l_I_m,dtype=float)
|
||||
x0 = np.concatenate([initial_l,initial_parameters(problem,initial_l)])
|
||||
r0 = _free_residual(problem,x0)
|
||||
fit = least_squares(
|
||||
lambda value:_free_residual(problem,value),x0,jac='2-point',
|
||||
jac_sparsity=_free_sparsity(problem,x0),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=max_nfev,x_scale='jac',
|
||||
ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
detail = {}
|
||||
rf = _free_residual(problem,fit.x,detail)
|
||||
information = _marginal_lever_information(fit.jac)
|
||||
_,singular_values,Vt = np.linalg.svd(information)
|
||||
tolerance = max(singular_values[0]*1e-9,1e-10)
|
||||
rank = int(np.sum(singular_values>tolerance))
|
||||
covariance = np.linalg.pinv(information,rcond=1e-9)
|
||||
dof = max(_effective_residual_dimension(detail)-len(fit.x),1)
|
||||
condition = (float(singular_values[0]/singular_values[-1])
|
||||
if singular_values[-1]>tolerance else np.inf)
|
||||
return FreeLeverResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
|
||||
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),
|
||||
final_cost=.5*float(np.dot(rf,rf)),nfev=int(fit.nfev),
|
||||
optimality=float(fit.optimality),chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
position_residual_m=_vector_stats(detail.get('position_physical',[])),
|
||||
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
|
||||
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_information_condition_number=condition,lever_precision_rank=rank,
|
||||
weakest_lever_direction_I=Vt[-1].copy())
|
||||
|
||||
|
||||
def solve_free_lever_many(problems,initial_l_I_m,max_nfev=120,
|
||||
initial_state_values=None,lever_prior_mean_m=None,
|
||||
lever_prior_covariance_m2=None,return_state_values=False):
|
||||
problems=tuple(problems)
|
||||
initial_l=np.asarray(initial_l_I_m,dtype=float)
|
||||
sizes=[NODE_DOF*len(problem.segment.nodes) for problem in problems]
|
||||
offsets=np.cumsum([3,*sizes])
|
||||
states=([initial_parameters(problem,initial_l) for problem in problems]
|
||||
if initial_state_values is None else
|
||||
[np.asarray(value,dtype=float) for value in initial_state_values])
|
||||
prior_mean=(None if lever_prior_mean_m is None else
|
||||
np.asarray(lever_prior_mean_m,dtype=float))
|
||||
prior_cov=(None if lever_prior_covariance_m2 is None else
|
||||
np.asarray(lever_prior_covariance_m2,dtype=float))
|
||||
prior_whitener=(None if prior_cov is None else
|
||||
np.linalg.inv(np.linalg.cholesky(prior_cov)))
|
||||
x0=np.concatenate([initial_l,*states])
|
||||
def evaluate(value,details=None):
|
||||
chunks=[]
|
||||
for index,problem in enumerate(problems):
|
||||
local={} if details is not None else None
|
||||
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
|
||||
details=local,lever_override=value[:3]))
|
||||
if details is not None:
|
||||
for key,items in local.items(): details.setdefault(key,[]).extend(items)
|
||||
if prior_whitener is not None:
|
||||
prior_error=prior_whitener@(value[:3]-prior_mean)
|
||||
chunks.append(prior_error)
|
||||
if details is not None:
|
||||
details.setdefault('lever_prior',[]).extend(prior_error.tolist())
|
||||
return np.concatenate(chunks)
|
||||
r0=evaluate(x0)
|
||||
sparsity=lil_matrix((len(r0),len(x0)),dtype=int)
|
||||
row=0; row_offsets=[0]
|
||||
for index,problem in enumerate(problems):
|
||||
local=jacobian_sparsity(problem,states[index])
|
||||
sparsity[row:row+local.shape[0],:3]=1
|
||||
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]]=local
|
||||
row+=local.shape[0]
|
||||
row_offsets.append(row)
|
||||
if prior_whitener is not None:
|
||||
sparsity[row:row+3,:3]=1
|
||||
fit=least_squares(
|
||||
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
|
||||
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
detail={}
|
||||
rf=evaluate(fit.x,detail)
|
||||
information=_additive_marginal_lever_information(
|
||||
fit.jac,row_offsets,offsets)
|
||||
if prior_cov is not None:
|
||||
information+=np.linalg.inv(prior_cov)
|
||||
_,singular_values,Vt=np.linalg.svd(information)
|
||||
tolerance=max(singular_values[0]*1e-9,1e-10)
|
||||
rank=int(np.sum(singular_values>tolerance))
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
dof=max(_effective_residual_dimension(detail)-len(fit.x),1)
|
||||
condition=(float(singular_values[0]/singular_values[-1])
|
||||
if singular_values[-1]>tolerance else np.inf)
|
||||
result=FreeLeverResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
|
||||
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
position_residual_m=_vector_stats(detail.get('position_physical',[])),
|
||||
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
|
||||
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_information_condition_number=condition,lever_precision_rank=rank,
|
||||
weakest_lever_direction_I=Vt[-1].copy())
|
||||
if return_state_values:
|
||||
states=[fit.x[offsets[i]:offsets[i+1]].copy()
|
||||
for i in range(len(problems))]
|
||||
return result,states
|
||||
return result
|
||||
Reference in New Issue
Block a user