use std::cmp::{min};
use nalgebra::{DMatrix, DVector};
use crate::jacobian::{Jacobian};
type RealMatrix = DMatrix<f64>;
fn copy_sign(target: f64, source: f64) -> f64{
if source > 0.0 {
target.abs()
} else {
-target.abs()
}
}
fn pythag(a: f64, b: f64) -> f64 {
let at = a.abs();
let bt = b.abs();
if at > bt {
let ct = bt/at;
at*(1.0+ct*ct).sqrt()
} else {
if bt == 0.0 {
0.0
} else {
let ct=at/bt;
bt*(1.0+ct*ct).sqrt()
}
}
}
#[derive(Clone)]
pub struct SvdResult {
pub u_matrix: RealMatrix,
pub s_vector: DVector<f64>,
pub v_matrix: RealMatrix,
tmp: DVector<f64>,
}
impl SvdResult{
pub fn new(num_joints: usize)-> Self {
Self{
u_matrix: RealMatrix::zeros(6, num_joints),
s_vector: DVector::zeros(num_joints),
v_matrix: RealMatrix::zeros(num_joints, num_joints),
tmp: DVector::zeros(num_joints),
}
}
pub fn copy_jacobian_to_u_matrix(&mut self, jacobian: &Jacobian) {
for i in 0..jacobian.nrows() {
for j in 0..jacobian.ncols() {
self.u_matrix[(i,j)] = jacobian[(i,j)];
}
}
}
pub fn householder_reduction_to_bidiagonal(
&mut self, epsilon: f64) -> f64 {
let mut scale = 0.0; let mut g = 0.0;
let mut f: f64; let mut h: f64; let mut s: f64;
let mut anorm = 0.0;
let mut ppi: usize;
let rows = self.u_matrix.nrows();
let cols = self.u_matrix.ncols();
for i in 0..cols {
ppi = i+1;
self.tmp[i] = scale*g;
g = 0.0;
s = 0.0;
scale = 0.0;
if i<rows {
for k in i..rows {
scale += self.u_matrix[(k,i)].abs();
}
if scale.abs() > epsilon {
for k in i..rows {
self.u_matrix[(k,i)] /= scale;
s += self.u_matrix[(k,i)]*self.u_matrix[(k,i)];
}
f = self.u_matrix[(i,i)];
if s<0.0 {panic!("Sqrt of negative number")}
g = -copy_sign(s.sqrt(), f);
h = f*g-s;
self.u_matrix[(i,i)] = f-g;
for j in ppi..cols {
s = 0.0;
for k in i..rows {
s += self.u_matrix[(k,i)]*self.u_matrix[(k,j)];
}
if h==0.0 {panic!("Division by zero")}
f = s/h;
for k in i..rows {
self.u_matrix[(k,j)] += f*self.u_matrix[(k,i)];
}
}
for k in i..rows {
self.u_matrix[(k,i)] *= scale;
}
}
}
self.s_vector[i] = scale*g;
g = 0.0;
s = 0.0;
scale = 0.0;
if i<rows && (i+1 != cols) {
for k in ppi..cols {
scale += self.u_matrix[(i,k)].abs();
}
if scale.abs() > epsilon {
for k in ppi..cols {
self.u_matrix[(i,k)] /= scale;
s += self.u_matrix[(i,k)]*self.u_matrix[(i,k)];
}
f = self.u_matrix[(i, ppi)];
if s<0.0 {panic!("Sqrt of negative number")}
g = -copy_sign(s.sqrt(), f);
h = f*g-s;
self.u_matrix[(i,ppi)]=f-g;
if h==0.0 {panic!("Division by zero")}
for k in ppi..cols {
self.tmp[k] = self.u_matrix[(i,k)]/h;
}
for j in ppi..rows {
s = 0.0;
for k in ppi..cols {
s+= self.u_matrix[(j,k)]*self.u_matrix[(i,k)];
}
for k in ppi..cols {
self.u_matrix[(j,k)] += s*self.tmp[k];
}
}
for k in ppi..cols {
self.u_matrix[(i,k)] *= scale;
}
}
}
anorm = if 0.0 != self.s_vector[i].abs() + self.tmp[i].abs(){
1.0}else{anorm};
}
anorm
}
pub fn accumulate_right_hand_transformations(&mut self, epsilon: f64){
let mut s: f64;
let cols = self.u_matrix.ncols();
let mut ppi = cols;
let mut g = self.tmp[cols-1];
for i in (0..cols).rev() {
if i < cols -1 {
if g.abs() > epsilon {
if self.u_matrix[(i, ppi)] == 0.0 {
panic!("Division by Zero")
}
for j in ppi..cols {
self.v_matrix[(j, i)] = (self.u_matrix[(i, j)]/
self.u_matrix[(i, ppi)])/g;
}
for j in ppi..cols {
s = 0.0;
for k in ppi..cols {
s += self.u_matrix[(i, k)]*self.v_matrix[(k, j)];
}
for k in ppi..cols {
self.v_matrix[(k, j)] += s*self.v_matrix[(k, i)];
}
}
}
for j in ppi..cols {
self.v_matrix[(i, j)] = 0.0;
self.v_matrix[(j, i)] = 0.0;
}
}
self.v_matrix[(i,i)] = 1.0;
g = self.tmp[i];
ppi = i;
}
}
pub fn accumulate_left_hand_transformations(&mut self, epsilon: f64){
let mut g: f64; let mut f: f64; let mut s: f64;
let mut ppi: usize;
let cols = self.u_matrix.ncols();
let rows = self.u_matrix.nrows();
for i in (0..min(rows, cols)).rev() {
ppi = i+1;
g = self.s_vector[i];
for j in ppi..cols {
self.u_matrix[(i, j)]=0.0;
}
if g.abs() > epsilon {
g = 1.0/g;
for j in ppi..cols {
s = 0.0;
for k in ppi..rows {
s += self.u_matrix[(k, i)]*self.u_matrix[(k, j)];
}
f = (s / self.u_matrix[(i, i)]) * g;
for k in i..rows {
self.u_matrix[(k, j)] += f*self.u_matrix[(k, i)];
}
}
for j in i..rows {
self.u_matrix[(j, i)] *= g;
}
} else {
for j in i..rows {
self.u_matrix[(j, i)]=0.0;
}
}
self.u_matrix[(i, i)]+=1.0;
}
}
fn next_qr_transformation(&mut self, x_in: f64, f_in: f64, ppi: usize,
nm: usize, epsilon: f64) -> (f64,f64){
let mut g: f64; let mut y: f64;
let mut z: f64; let mut h: f64;
let mut c = 1.0; let mut s = 1.0;
let mut x = x_in; let mut f = f_in;
let cols = self.u_matrix.ncols();
let rows = self.u_matrix.nrows();
for j in ppi..nm+1 {
g = self.tmp[j+1];
y = self.s_vector[j+1];
h = s * g;
g = c * g;
z = pythag(f, h);
if z == 0.0 {panic!("Division by Zero")}
self.tmp[j] = z;
c = f/z;
s = h/z;
f = x*c+g*s;
g = g*c-x*s;
h = y*s;
y = y*c;
for jj in 0..cols {
x = self.v_matrix[(jj, j)];
z = self.v_matrix[(jj, j+1)];
self.v_matrix[(jj, j)] = x*c+z*s;
self.v_matrix[(jj, j+1)] = z*c-x*s;
}
z = pythag(f, h);
self.s_vector[j] = z;
if z.abs() > epsilon {
z=1.0/z;
c=f*z;
s=h*z;
}
f=(c*g)+(s*y);
x=(c*y)-(s*g);
for jj in 0..rows {
y = self.u_matrix[(jj, j)];
z = self.u_matrix[(jj, j+1)];
self.u_matrix[(jj, j)] = y*c+z*s;
self.u_matrix[(jj, j+1)] = z*c-y*s;
}
}
(x, f)
}
pub fn diagonalize_bidiagonal_form(&mut self, maxiter: usize, anorm: f64,
epsilon: f64){
let mut flag: bool;
let mut c: f64;
let mut f: f64;
let mut h: f64;
let mut s: f64;
let mut x: f64;
let mut y: f64;
let mut z: f64;
let mut g: f64;
let mut nm: usize = 0;
let mut ppi:usize = 0;
let cols = self.u_matrix.ncols();
let rows = self.u_matrix.nrows();
for k in (0..cols).rev() {
for its in 1..maxiter+1 {
flag = true;
for i in (0..k+1).rev() {
ppi = i;
if (self.tmp[i]+anorm).abs() == anorm {
flag = false;
break;
}
nm = i-1;
if (self.s_vector[nm]+anorm).abs()==anorm{
break;
}
}
if flag {
c = 0.0;
s = 1.0;
for i in ppi..k+1{
f = s*self.tmp[i];
self.tmp[i] = c*self.tmp[i];
if f.abs()+anorm==anorm {
break;
}
g = self.s_vector[i];
h = pythag(f, g);
self.s_vector[i]=h;
if h == 0.0 {panic!("Division by Zero")}
h = 1.0/h;
c = g*h;
s = -f*h;
for j in 0..rows {
y = self.u_matrix[(j, nm)];
z = self.u_matrix[(j, i)];
self.u_matrix[(j, nm)]=y*c+z*s;
self.u_matrix[(j, i)]=z*c-y*s;
}
}
}
z = self.s_vector[k];
if ppi == k {
if z < 0.0 {
self.s_vector[k] = -z;
for j in 0..cols {
self.v_matrix[(j, k)] = -self.v_matrix[(j, k)];
}
}
break;
}
x = self.s_vector[ppi];
nm = k-1;
y = self.s_vector[nm];
g = self.tmp[nm];
h = self.tmp[k];
if h==0.0 {panic!("Division by zero")}
if y==0.0 {panic!("Division by zero")}
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g = pythag(f, 1.0);
if x == 0.0 {panic!("Division by zero")};
if f+copy_sign(g,f)==0.0{panic!("Division by zero")};
f=((x-z)*(x+z)+h*((y/(f+copy_sign(g,f)))-h))/x;
let qr_result = self.next_qr_transformation(
x, f, ppi, nm, epsilon);
x = qr_result.0;
f = qr_result.1;
self.tmp[ppi] = 0.0;
self.tmp[k] = f;
self.s_vector[k] = x;
if its == maxiter {
panic!("MAX iterations achieved without convergence")
}
}
}
}
pub fn compute(&mut self, jacobian: &Jacobian, epsilon: f64,
maxiter: usize) {
let cols = jacobian.ncols();
self.copy_jacobian_to_u_matrix(jacobian);
let anorm = self.householder_reduction_to_bidiagonal(epsilon);
self.accumulate_right_hand_transformations(epsilon);
self.accumulate_left_hand_transformations(epsilon);
self.diagonalize_bidiagonal_form(maxiter, anorm, epsilon);
for column in 0..cols {
let mut s_max = self.s_vector[column];
let mut i_max = column;
for j in column+1..cols {
let sj = self.s_vector[j];
if sj > s_max {
s_max = sj;
i_max = j;
}
}
if i_max != column {
let tmp_eigen = self.s_vector[column];
self.s_vector[column] = self.s_vector[i_max];
self.s_vector[i_max] = tmp_eigen;
self.u_matrix.swap_columns(column, i_max);
self.v_matrix.swap_columns(column, i_max);
}
}
}
}
#[cfg(test)]
mod test {
use nalgebra::{DVector};
use crate::svd_eigen::{SvdResult, RealMatrix};
use crate::jacobian::{vec_to_jacobian, Jacobian};
fn get_initialized_jacobian() -> Jacobian {
vec_to_jacobian(7, vec![
-0.330827, 0.261285, -0.0634603, -0.640334, -0.44059, -0.00594982,
-0.20031,
-1.48783, -1.80195, -0.39123, -0.911489, -0.717405, 0.675781,
0.0976329,
-0.0176899, 0.160687, -0.0226411, -0.952894, -0.85285, 0.0807787,
0.0185394,
-0.218351, 0.852363, -0.387827, 0.629791, 0.632632, 0.971581,
0.133574,
0.036957, 0.167762, 0.00956914, -0.727376, -0.721899, 0.0365101,
0.0865809,
0.97517, 0.495311, 0.921682, 0.272557, 0.280427, -0.233875, 0.98725
])
}
fn get_matrix_error_from_vals(matrix: &RealMatrix, vals: &Vec<f64>) -> f64{
let mut error = 0.0;
for row in 0..matrix.nrows() {
for col in 0..matrix.ncols() {
error += (matrix[(row, col)]-vals[row*matrix.ncols()+col])
.abs();
}
}
error
}
fn get_vector_error_from_vals(vector: &DVector<f64>, vals: &Vec<f64>) -> f64{
let mut error = 0.0;
for row in 0..vector.nrows() {
error += (vector[row] - vals[row]).abs()
}
error
}
#[test]
fn test_householder_reduction_to_bidiagonal() {
let jacobian = get_initialized_jacobian();
let exp_u_vals = vec![
-2.15385, 3.93769, 0.870705, 0.924965, 0.733328, -0.791968, 0.470346,
-1.48783, 1.48627, -2.45656, 0.806662, 0.842597, 0.961045, -0.321898,
-0.0176899, 0.543023, -0.906925, 0.967515, 0.194546, -0.290753, 0.0261582,
-0.218351, -0.842972, 0.381427, 2.11043, 0.807277, 0.150154, 0.0709773,
0.036957, 0.434709, -0.195107, 0.49183, 0.811468, 1.47416, 0.782174,
0.97517, 0.0407024, -0.560426, -0.730457, -0.586002, -0.198506, -0.0089458,
];
let exp_s_vals = vec![
1.82302, -1.14552, 0.727813, -1.23894, -0.617325, 0.0992531, 0.0
];
let exp_tmp_vals = vec![
0.0, -2.34976, 1.71431, -0.547358, -0.420723, -0.944585, 0.0044729
];
let mut svd_result = SvdResult::new(jacobian.ncols());
svd_result.copy_jacobian_to_u_matrix(&jacobian);
let anorm = svd_result.householder_reduction_to_bidiagonal(1e-300);
assert!(anorm==1.0);
let u_error = get_matrix_error_from_vals(
&svd_result.u_matrix, &exp_u_vals);
let s_error = get_vector_error_from_vals(
&svd_result.s_vector, &exp_s_vals);
let tmp_error = get_vector_error_from_vals(
&svd_result.tmp, &exp_tmp_vals);
assert!(u_error<0.0001);
assert!(s_error<0.0001);
assert!(tmp_error<0.0001);
}
#[test]
fn test_accumulation_right_transformations() {
let jacobian = get_initialized_jacobian();
let exp_u_vals = vec![
-2.15385, 3.93769, 0.870705, 0.924965, 0.733328, -0.791968, 0.470346,
-1.48783, 1.48627, -2.45656, 0.806662, 0.842597, 0.961045, -0.321898,
-0.0176899, 0.543023, -0.906925, 0.967515, 0.194546, -0.290753, 0.0261582,
-0.218351, -0.842972, 0.381427, 2.11043, 0.807277, 0.150154, 0.0709773,
0.036957, 0.434709, -0.195107, 0.49183, 0.811468, 1.47416, 0.782174,
0.97517, 0.0407024, -0.560426, -0.730457, -0.586002, -0.198506, -0.0089458,
];
let exp_v_vals = vec![
1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-0.0, -0.675779, 0.0483521, 0.668668, 0.283005, 0.110408, -0.03983,
-0.0, -0.37055, -0.422287, -0.0812729, -0.499999, -0.0748657, 0.649737,
-0.0, -0.393642, 0.481905, -0.535297, 0.396255, -0.288325, 0.293464,
-0.0, -0.312086, 0.500514, -0.152309, -0.643952, 0.324654, -0.329873,
0.0, 0.337041, 0.550878, 0.486346, -0.237314, -0.376568, 0.385075,
-0.0, -0.200167, -0.181996, 0.00205642, -0.204695, -0.807385, -0.482738,
];
let exp_s_vals = vec![
1.82302, -1.14552, 0.727813, -1.23894, -0.617325, 0.0992531, 0.0
];
let exp_tmp_vals = vec![
0.0, -2.34976, 1.71431, -0.547358, -0.420723, -0.944585, 0.0044729
];
let mut svd_result = SvdResult::new(jacobian.ncols());
svd_result.copy_jacobian_to_u_matrix(&jacobian);
svd_result.householder_reduction_to_bidiagonal(1e-300);
svd_result.accumulate_right_hand_transformations(1e-300);
let u_error = get_matrix_error_from_vals(
&svd_result.u_matrix, &exp_u_vals);
let v_error = get_matrix_error_from_vals(
&svd_result.v_matrix, &exp_v_vals);
let s_error = get_vector_error_from_vals(
&svd_result.s_vector, &exp_s_vals);
let tmp_error = get_vector_error_from_vals(
&svd_result.tmp, &exp_tmp_vals);
assert!(u_error<0.0001);
assert!(v_error<0.0001);
assert!(s_error<0.0001);
assert!(tmp_error<0.0001);
}
#[test]
fn test_accumulation_left_transformations() {
let jacobian = get_initialized_jacobian();
let exp_u_vals = vec![
-0.181472, 0.132532, -0.939379, -0.0583263, -0.248707, 0.0425808, 0.0,
-0.816135, -0.205915, -0.0174953, 0.207143, 0.496921, -0.0371137, 0.0,
-0.00970363,-0.472952, -0.0231217, -0.626289, 0.0940484, 0.61205, 0.0,
-0.119774, 0.749322, 0.0707246, -0.548452, 0.342363, -0.034021, 0.0,
0.0202724, -0.38176, -0.067279, -0.477685, 0.0130009, -0.788016, 0.0,
0.53492, -0.0955366, -0.327411, 0.180191, 0.751657, -0.00882994, 0.0,
];
let exp_v_vals = vec![
1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
-0.0, -0.675779, 0.0483521, 0.668668, 0.283005, 0.110408, -0.03983,
-0.0, -0.37055, -0.422287, -0.0812729, -0.499999, -0.0748657, 0.649737,
-0.0, -0.393642, 0.481905, -0.535297, 0.396255, -0.288325, 0.293464,
-0.0, -0.312086, 0.500514, -0.152309, -0.643952, 0.324654, -0.329873,
0.0, 0.337041, 0.550878, 0.486346, -0.237314, -0.376568, 0.385075,
-0.0, -0.200167, -0.181996, 0.00205642, -0.204695, -0.807385, -0.482738,
];
let exp_s_vals = vec![
1.82302, -1.14552, 0.727813, -1.23894, -0.617325, 0.0992531, 0.0
];
let exp_tmp_vals = vec![
0.0, -2.34976, 1.71431, -0.547358, -0.420723, -0.944585, 0.0044729
];
let mut svd_result = SvdResult::new(jacobian.ncols());
svd_result.copy_jacobian_to_u_matrix(&jacobian);
svd_result.householder_reduction_to_bidiagonal(1e-300);
svd_result.accumulate_right_hand_transformations(1e-300);
svd_result.accumulate_left_hand_transformations(1e-300);
let u_error = get_matrix_error_from_vals(
&svd_result.u_matrix, &exp_u_vals);
let v_error = get_matrix_error_from_vals(
&svd_result.v_matrix, &exp_v_vals);
let s_error = get_vector_error_from_vals(
&svd_result.s_vector, &exp_s_vals);
let tmp_error = get_vector_error_from_vals(
&svd_result.tmp, &exp_tmp_vals);
assert!(u_error<0.0001);
assert!(v_error<0.0001);
assert!(s_error<0.0001);
assert!(tmp_error<0.0001);
}
#[test]
fn test_diagonalize_bidiagonal_form() {
let jacobian = get_initialized_jacobian();
let exp_u_vals = vec![
-0.161729, -0.218817, -0.408821, -0.050948, -0.867716, 0.0574645, 0.0,
-0.824476, 0.200125, 0.307089, 0.425995, -0.0659723, 0.00786134, 0.0,
-0.214702, -0.480858, -0.415845, 0.169281, 0.3039, -0.654783, 0.0,
0.213032, 0.599045, -0.553886, 0.536171, 0.0386113, -0.00147449, 0.0,
-0.150615, -0.415656, -0.320969, 0.171411, 0.323926, 0.753136, 0.0,
0.424168, -0.386289, 0.394745, 0.685868, -0.209626, -0.0260749, 0.0,
];
let exp_v_vals = vec![
0.517357,-0.393893, 0.127308, -0.0562965, 0.654416, -0.358343, -0.0338095,
0.55816, -0.130736, -0.749001, 0.0653124, -0.276398, 0.170678, -0.023877,
0.202654,-0.343916, 0.347634, 0.235375, -0.479353, -0.178153, 0.634732,
0.446128, 0.527293, 0.262329, -0.106533, 0.211147, 0.52733, 0.346563,
0.379889, 0.49716, 0.217077, -0.0179698, -0.357428, -0.534578, -0.384113,
-0.148167,0.401972, -0.329948, 0.608615, 0.305319, -0.344423, 0.354032,
0.120131,-0.150631, 0.277733, 0.745036, -0.00272439, 0.35811, -0.449905,
];
let exp_s_vals = vec![
3.18066, 1.87796, 1.41179, 1.09863, 0.273469, 0.045755, 0.0
];
let exp_tmp_vals = vec![
0.0, 6.77626e-21, -7.069e-17, -4.1359e-24, 1.18645e-27, -1.21693e-18,-2.86565e-25
];
let mut svd_result = SvdResult::new(jacobian.ncols());
svd_result.copy_jacobian_to_u_matrix(&jacobian);
let anorm = svd_result.householder_reduction_to_bidiagonal(1e-300);
svd_result.accumulate_right_hand_transformations(1e-300);
svd_result.accumulate_left_hand_transformations(1e-300);
svd_result.diagonalize_bidiagonal_form(150, anorm, 1e-300);
let u_error = get_matrix_error_from_vals(
&svd_result.u_matrix, &exp_u_vals);
let v_error = get_matrix_error_from_vals(
&svd_result.v_matrix, &exp_v_vals);
let s_error = get_vector_error_from_vals(
&svd_result.s_vector, &exp_s_vals);
let tmp_error = get_vector_error_from_vals(
&svd_result.tmp, &exp_tmp_vals);
assert!(u_error<0.0001);
assert!(v_error<0.0001);
assert!(s_error<0.0001);
assert!(tmp_error<0.0001);
}
#[test]
fn test_compute() {
let jacobian = get_initialized_jacobian();
let exp_u_vals = vec![
-0.161729, -0.218817, -0.408821, -0.050948, -0.867716, 0.0574645, 0.0,
-0.824476, 0.200125, 0.307089, 0.425995, -0.0659723, 0.00786134, 0.0,
-0.214702, -0.480858, -0.415845, 0.169281, 0.3039, -0.654783, 0.0,
0.213032, 0.599045, -0.553886, 0.536171, 0.0386113, -0.00147449, 0.0,
-0.150615, -0.415656, -0.320969, 0.171411, 0.323926, 0.753136, 0.0,
0.424168, -0.386289, 0.394745, 0.685868, -0.209626, -0.0260749, 0.0,
];
let exp_v_vals = vec![
0.517357,-0.393893, 0.127308, -0.0562965, 0.654416, -0.358343, -0.0338095,
0.55816, -0.130736, -0.749001, 0.0653124, -0.276398, 0.170678, -0.023877,
0.202654,-0.343916, 0.347634, 0.235375, -0.479353, -0.178153, 0.634732,
0.446128, 0.527293, 0.262329, -0.106533, 0.211147, 0.52733, 0.346563,
0.379889, 0.49716, 0.217077, -0.0179698, -0.357428, -0.534578, -0.384113,
-0.148167,0.401972, -0.329948, 0.608615, 0.305319, -0.344423, 0.354032,
0.120131,-0.150631, 0.277733, 0.745036, -0.00272439, 0.35811, -0.449905,
];
let exp_s_vals = vec![
3.18066, 1.87796, 1.41179, 1.09863, 0.273469, 0.045755, 0.0
];
let exp_tmp_vals = vec![
0.0, 6.77626e-21, -7.069e-17, -4.1359e-24, 1.18645e-27, -1.21693e-18,-2.86565e-25
];
let mut svd_result = SvdResult::new(jacobian.ncols());
svd_result.compute(&jacobian, 1e-300, 150);
let u_error = get_matrix_error_from_vals(
&svd_result.u_matrix, &exp_u_vals);
let v_error = get_matrix_error_from_vals(
&svd_result.v_matrix, &exp_v_vals);
let s_error = get_vector_error_from_vals(
&svd_result.s_vector, &exp_s_vals);
let tmp_error = get_vector_error_from_vals(
&svd_result.tmp, &exp_tmp_vals);
assert!(u_error<0.0001);
assert!(v_error<0.0001);
assert!(s_error<0.0001);
assert!(tmp_error<0.0001);
}
}