530 lines
24 KiB
Python
530 lines
24 KiB
Python
'''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
|