335 lines
11 KiB
Java
335 lines
11 KiB
Java
import com.imdroid.sideslope.bd.FocusCalculator;
|
|
import com.imdroid.sideslope.bd.FocusCalculator1;
|
|
import com.imdroid.sideslope.bd.FocusCalculator2;
|
|
import com.imdroid.sideslope.bd.FocusCalculator5;
|
|
import org.ejml.simple.SimpleMatrix;
|
|
import org.junit.Test;
|
|
import org.junit.runner.RunWith;
|
|
import org.springframework.boot.test.context.SpringBootTest;
|
|
import org.springframework.test.context.junit4.SpringRunner;
|
|
|
|
import java.io.BufferedReader;
|
|
import java.io.FileReader;
|
|
import java.io.IOException;
|
|
import java.text.ParseException;
|
|
import java.text.SimpleDateFormat;
|
|
import java.time.LocalDateTime;
|
|
import java.util.ArrayList;
|
|
import java.util.Date;
|
|
import java.util.List;
|
|
import java.util.ListIterator;
|
|
|
|
@RunWith(SpringRunner.class)
|
|
@SpringBootTest(
|
|
classes = {
|
|
FocusCalculatorTest.class,
|
|
FocusCalculator1.class,
|
|
FocusCalculator2.class,
|
|
FocusCalculator5.class
|
|
}
|
|
)
|
|
|
|
public class FocusCalculatorTest {
|
|
class CalcResult{
|
|
double[] xyz;
|
|
long time;
|
|
boolean enabled = true;
|
|
};
|
|
|
|
final double xyTreshold = 3;
|
|
final double zTreshold = 3;
|
|
|
|
List<CalcResult> calcResultList = new ArrayList<>();
|
|
List<CalcResult> filterResults = new ArrayList<>();
|
|
|
|
FocusCalculator focusCalculator;
|
|
String dataFileName = "src/test/java/data42.txt";
|
|
|
|
@Test
|
|
public void run() throws IOException, ParseException {
|
|
System.out.println("*********** 2307042 *************");
|
|
/*System.out.println("算法1");
|
|
focusCalculator = new FocusCalculator1();
|
|
test();
|
|
System.out.println("算法2");
|
|
focusCalculator = new FocusCalculator2();
|
|
test();*/
|
|
System.out.println("算法3");
|
|
focusCalculator = new FocusCalculator5();
|
|
((FocusCalculator5)focusCalculator).setParas("2307042",50, (short) 20);
|
|
test();
|
|
}
|
|
|
|
|
|
public void test() throws IOException, ParseException {
|
|
FileReader fr = new FileReader(dataFileName);
|
|
BufferedReader br = new BufferedReader(fr);
|
|
String line = "";
|
|
String[] arrs = null;
|
|
SimpleDateFormat sdf = new SimpleDateFormat("yyyy/MM/dd HH:mm:ss");
|
|
|
|
long lastTime = 0;
|
|
double[] last = null;
|
|
|
|
while ((line = br.readLine()) != null) {
|
|
arrs = line.split("\t");
|
|
Date time = sdf.parse(arrs[0]+" "+arrs[1]);
|
|
if(lastTime==0) lastTime = time.getTime();
|
|
|
|
if(Math.abs(time.getTime() - lastTime)>30*60*1000) {
|
|
last = calcGravity(last, lastTime);
|
|
lastTime = time.getTime();
|
|
//if(calcResultList.size()>=36) break;//18个小时
|
|
}
|
|
focusCalculator.addXyz(new double[]{
|
|
Double.parseDouble(arrs[2])*10,
|
|
Double.parseDouble(arrs[3])*10,
|
|
Double.parseDouble(arrs[4])*10,2},LocalDateTime.now());
|
|
}
|
|
br.close();
|
|
fr.close();
|
|
|
|
//if(focusCalculator.getB562Num()>0) {
|
|
calcGravity(last, lastTime);
|
|
//}
|
|
|
|
System.out.println("raw results statistic:");
|
|
calcStd(calcResultList);
|
|
|
|
System.out.println("4 hours filter, average");
|
|
runFilter(4);
|
|
calcStd(filterResults);
|
|
filterResults.removeAll(filterResults);
|
|
/*
|
|
System.out.println("4 hours filter, igg3");
|
|
runFilter2(4);
|
|
calcStd(filterResults);
|
|
filterResults.removeAll(filterResults);
|
|
|
|
System.out.println("8 hours filter, average");
|
|
runFilter(8);
|
|
calcStd(filterResults);
|
|
filterResults.removeAll(filterResults);
|
|
System.out.println("8 hours filter, igg3");
|
|
runFilter2(8);
|
|
calcStd(filterResults);
|
|
filterResults.removeAll(filterResults);
|
|
*/
|
|
calcResultList.removeAll(calcResultList);
|
|
|
|
}
|
|
|
|
double[] calcGravity(double[] last, long time){
|
|
focusCalculator.setReferPoint(last);
|
|
double[] result = focusCalculator.resultB562();
|
|
// 解算结果处理
|
|
CalcResult calcResult = new CalcResult();
|
|
calcResult.xyz = result;
|
|
calcResult.time = time;
|
|
if(last!=null){
|
|
//跳点
|
|
if(Math.abs(result[0]-last[0])>xyTreshold ||
|
|
Math.abs(result[1]-last[1])>xyTreshold ||
|
|
Math.abs(result[2]-last[2])>zTreshold){
|
|
calcResult.enabled = false;
|
|
}
|
|
}
|
|
calcResultList.add(calcResult);
|
|
focusCalculator.reset();
|
|
return result;
|
|
}
|
|
|
|
void runFilter(int filterHour){
|
|
long filterWin = filterHour*3600*1000;
|
|
|
|
for(int i=0; i<calcResultList.size(); i++){
|
|
CalcResult result = calcResultList.get(i);
|
|
if(!result.enabled) continue;
|
|
|
|
double[] filterXyz = new double[]{result.xyz[0],result.xyz[1],result.xyz[2]};
|
|
long time = result.time;
|
|
ListIterator listIterator = calcResultList.listIterator(i);
|
|
int count = 1;
|
|
while(listIterator.hasPrevious()){
|
|
result = (CalcResult) listIterator.previous();
|
|
if(result.enabled) {
|
|
if (time - result.time <= filterWin) {
|
|
filterXyz[0] += result.xyz[0];
|
|
filterXyz[1] += result.xyz[1];
|
|
filterXyz[2] += result.xyz[2];
|
|
count++;
|
|
} else break;
|
|
}
|
|
}
|
|
filterXyz[0] /= count;
|
|
filterXyz[1] /= count;
|
|
filterXyz[2] /= count;
|
|
CalcResult filterResult = new CalcResult();
|
|
filterResult.xyz = filterXyz;
|
|
filterResult.time = time;
|
|
filterResults.add(filterResult);
|
|
}
|
|
}
|
|
|
|
// IGG3
|
|
void runFilter2(int filterHour){
|
|
long filterWin = filterHour*3600*1000;
|
|
List<Double> listE = new ArrayList<>();
|
|
List<Double> listN = new ArrayList<>();
|
|
List<Double> listD = new ArrayList<>();
|
|
|
|
for(int i=0; i<calcResultList.size(); i++){
|
|
CalcResult result = calcResultList.get(i);
|
|
if(!result.enabled) continue;
|
|
|
|
long time = result.time;
|
|
double[] filterXyz = new double[]{result.xyz[0],result.xyz[1],result.xyz[2]};
|
|
listE.add(result.xyz[0]);
|
|
listN.add(result.xyz[1]);
|
|
listD.add(result.xyz[2]);
|
|
ListIterator listIterator = calcResultList.listIterator(i);
|
|
while(listIterator.hasPrevious()){
|
|
result = (CalcResult) listIterator.previous();
|
|
if(result.enabled) {
|
|
if (time - result.time <= filterWin) {
|
|
listE.add(result.xyz[0]);
|
|
listN.add(result.xyz[1]);
|
|
listD.add(result.xyz[2]);
|
|
} else break;
|
|
}
|
|
}
|
|
|
|
if(listE.size() > 1){
|
|
double[] l = listE.stream().mapToDouble(Double::doubleValue).toArray();
|
|
filterXyz[0] = igg3(l);
|
|
l = listN.stream().mapToDouble(Double::doubleValue).toArray();
|
|
filterXyz[1] = igg3(l);
|
|
l = listD.stream().mapToDouble(Double::doubleValue).toArray();
|
|
filterXyz[2] = igg3(l);
|
|
}
|
|
|
|
CalcResult filterResult = new CalcResult();
|
|
filterResult.xyz = filterXyz;
|
|
filterResult.time = time;
|
|
filterResults.add(filterResult);
|
|
listE.removeAll(listE);
|
|
listN.removeAll(listN);
|
|
listD.removeAll(listD);
|
|
}
|
|
}
|
|
|
|
void calcStd(List<CalcResult> list){
|
|
double[] first = list.get(0).xyz;
|
|
double[] avg = {0,0,0};
|
|
double[] max = {first[0],first[1],first[2]};
|
|
double[] min = {first[0],first[1],first[2]};
|
|
int count = 0;
|
|
|
|
SimpleDateFormat format = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss"); //设置格式
|
|
|
|
// 求均值
|
|
for(CalcResult result:list){
|
|
//System.out.println(format.format(result.time)+
|
|
// " e:"+result.xyz[0]+" n:"+result.xyz[1]+" d:"+result.xyz[2]+
|
|
// " valid:"+result.enabled);
|
|
|
|
avg[0] += result.xyz[0];
|
|
avg[1] += result.xyz[1];
|
|
avg[2] += result.xyz[2];
|
|
|
|
if(result.xyz[0]>max[0]) max[0] = result.xyz[0];
|
|
if(result.xyz[1]>max[1]) max[1] = result.xyz[1];
|
|
if(result.xyz[2]>max[2]) max[2] = result.xyz[2];
|
|
|
|
if(result.xyz[0]<min[0]) min[0] = result.xyz[0];
|
|
if(result.xyz[1]<min[1]) min[1] = result.xyz[1];
|
|
if(result.xyz[2]<min[2]) min[2] = result.xyz[2];
|
|
|
|
if(!result.enabled) count++;
|
|
}
|
|
avg[0] /= list.size();
|
|
avg[1] /= list.size();
|
|
avg[2] /= list.size();
|
|
|
|
//求标准差
|
|
double[] std = {0,0,0};
|
|
for(CalcResult result:list){
|
|
std[0] += Math.abs(avg[0] - result.xyz[0]);
|
|
std[1] += Math.abs(avg[1] - result.xyz[1]);
|
|
std[2] += Math.abs(avg[2] - result.xyz[2]);
|
|
}
|
|
std[0] /= list.size();
|
|
std[1] /= list.size();
|
|
std[2] /= list.size();
|
|
System.out.println("total num: "+ list.size()+", bad num"+count);
|
|
System.out.println("std: {"+ std[0]+", "+std[1]+", "+std[2]+"}");
|
|
System.out.println("max delta: {"+ (max[0]-min[0])+", "+(max[1]-min[1])+", "+(max[2]-min[2])+"}");
|
|
}
|
|
|
|
public static double igg3(double[] l) {
|
|
SimpleMatrix L = new SimpleMatrix(l.length,1,false,l);
|
|
SimpleMatrix B = new SimpleMatrix(l.length,1);
|
|
B.fill(1);
|
|
SimpleMatrix P = new SimpleMatrix(l.length, l.length);
|
|
for(int i=0; i<l.length;i++){
|
|
//P.set(i,i,1/l[i]);
|
|
P.set(i,i,1);
|
|
}
|
|
double x_prev=0;
|
|
double x0=0;
|
|
for(int i=0;i<l.length;i++){
|
|
x0 += l[i];
|
|
}
|
|
x0 /= l.length;
|
|
|
|
L = L.minus(x0);
|
|
SimpleMatrix W,N,X,V,Q,Qvv;
|
|
SimpleMatrix sigma0;
|
|
double s0;
|
|
double k0=1.0, k1=5, k=1.0;
|
|
int iterNum = 0;
|
|
|
|
while(iterNum<10){
|
|
W = B.transpose().mult(P).mult(L); //W=B'*P*l;
|
|
N = B.transpose().mult(P).mult(B); //N=B'*P*B;
|
|
X = N.invert().mult(W); //x=inv(N)*W;% 待估参数向量
|
|
V = B.mult(X).minus(L); //v=B*x-l;% 残差向量
|
|
|
|
sigma0 = V.transpose().mult(P).mult(V).divide((l.length-1));
|
|
s0 = Math.sqrt(sigma0.get(0,0));//sigma0=sqrt(v'*P*v/(10-1));% 单位权中误差
|
|
//System.out.println(X);
|
|
//System.out.println(s0);
|
|
Q = P.invert();//Q=inv(P);
|
|
Qvv = Q.minus(B.mult(N.invert().mult(B.transpose())));//Qvv=Q-B*inv(N)*B';
|
|
|
|
for(int i=0; i<l.length; i++){
|
|
double v = V.get(i)/(s0*Math.sqrt(Qvv.get(i,i))); //v_=v(i)/(sigma0*sqrt(Qvv(i,i)));
|
|
if(Math.abs(v) < k0){
|
|
k = 1.0;
|
|
}
|
|
else if(Math.abs(v) > k1){
|
|
k = 1e-8;
|
|
}
|
|
else{
|
|
k=(k1-Math.abs(v))/(k1-k0);
|
|
k=k*k*(k0/Math.abs(v));
|
|
}
|
|
P.set(i,i,P.get(i,i)*k); //P(i,i)=P(i,i)*k;
|
|
}
|
|
|
|
if(x_prev==0) {
|
|
x_prev = X.get(0);
|
|
}
|
|
else if(Math.abs(x_prev-X.get(0))<0.05) {
|
|
break;
|
|
}
|
|
x_prev = X.get(0);
|
|
iterNum++;
|
|
}
|
|
|
|
//x=x0+x_prev;%求出的最后长度 初值+改正数
|
|
return (x0+x_prev);
|
|
}
|
|
|
|
}
|