最小二乗法の理論とその応用　東洋書店　田島稔・小牧和雄先生

```{r}

#5.7 三辺測量による測地網の平均

#p.200

P=matrix(c(39629.8,22374.87,16689.56,-13883.06,-6115.7,-46535.72),nrow=3)
P0=c(21777.8,-23583.4)
#P0=apply(P,2,mean)
diff=t(t(P)-P0)
S=sqrt(apply(diff^2,1,sum))
A=diff/S
Sobs=c(0.014,0.009,0.045)
Ps=(5*10^(-4))/(0.01^2+(S*2*10^(-6))^2)

ite=100
eta=0.1
xy_hat=P0

for(l in 1:ite){
xy_hat_pre=xy_hat  
dxy=solve(t(A)%*%diag(c(Ps))%*%A)%*%t(A)%*%diag(c(Ps))%*%Sobs
xy_hat=xy_hat*(1-eta)+eta*(P0+dxy)
print(sum(abs(xy_hat-xy_hat_pre)))
}

v=A%*%dxy-Sobs
  
```

```{r}

#p.233 条件方程式

#例題7.2

Lb=c(30,30,119)
P=diag(c(5000,1,2))


f=function(L){
return(as.numeric(c(Lb-L)%*%P%*%c(Lb-L)))
}

g=function(L){
return(sum(Lb+L)-180)  
}


La=c(2,3,5)
ite=3*10^3
eta=0.01
h=0.001

for(l in 1:ite){
W=g(rep(0,length(La)))
B=c()
for(j in 1:length(La)){
vec=La;vec_sub=vec;vec[j]=vec[j]+h
B=c(B,(g(vec)-g(vec_sub))/h)  
}
BPB=as.numeric(B%*%solve(P)%*%B)
V=-(solve(P)%*%B)*(BPB*W)
K=-(BPB*W)
La=La*(1-eta)+eta*(Lb+V)
print(f(La)-2*K*(sum(B*(La-Lb))+W))
}

#単位重量の観測の分散
m02=(W^2)/BPB

#分散共分散行列
sig=m02*(solve(P)-(solve(P)%*%B)%*%t(solve(P)%*%B)/(BPB))

```

```{r}

#p.249 例題7.3

f=function(L){
return(as.numeric(c(Lb-L)%*%P%*%c(Lb-L)))
}


Lb=c(12.113,25.825,37.018,39.125,15.82,27.906,2.114)
p=c(2.6,3.7,4.1,5.3,2.5,3.2,4)

B=matrix(c(1,0,0,0,1,0,0,0,1,0,0,-1,1,0,0,-1,-1,0,0,1,1),nrow=3)
W=B%*%Lb
P=diag(c(p))

La=rep(1,length(Lb))
ite=3*10^3
eta=0.01
h=0.001

for(l in 1:ite){
BPB=B%*%solve(P)%*%t(B)
V=-(solve(P)%*%t(B)%*%BPB%*%W)
K=-(BPB%*%W)
La=La*(1-eta)+eta*(Lb+V)
print(sum(f(La)-2*K*(B%*%(La-Lb)+W)))
}

#単位重量の観測の分散
m02=as.numeric(t(W)%*%solve(BPB)%*%W/nrow(B))

#分散共分散行列
sig=m02*(solve(P)-(solve(P)%*%t(B)%*%solve(BPB)%*%B%*%solve(P)))


```

```{r}

#p.268

A=matrix(c(1,0,1,0,1,1),nrow=3)
L=c(-15.253,15.249,0.004)

B=c(1,1)
W=0
mat=rbind(cbind(t(A)%*%A,B),c(B,0))
vec=c(-t(A)%*%L,-W)
param=solve(mat)%*%vec
X=param[1:ncol(A)]
K=param[-c(1:ncol(A))]
V=A%*%X+L
m02=sum(V^2)/(nrow(A)-ncol(A)+(nrow(mat)-ncol(A)))

```


```{r}

#p.277
#例題8.2 測地座標変換に関する問題

n=6 #Bの列幅
r=3 #Bの行幅
u=7 #Aの列幅

coord1=c(1,2,3) #x,y,z
coord2=c(3,5,4) #X,Y,Z

B=cbind(diag(rep(1,3)),diag(rep(-1,3)))
A=cbind(diag(rep(1,3)),matrix(c(coord1,0,coord1[3],-coord1[2],-coord1[3],0,coord1[1],coord1[2],-coord1[1],0),nrow=3))
W=coord1-coord2
P=diag(rep(1,n))


M=B%*%solve(P)%*%t(B)
AMA=t(A)%*%solve(M)%*%A
AMW=t(A)%*%solve(M)%*%W

#修正ガウス・ザイデル法
AA=-AMA　#行列のサンプル
C=AMW  #ベクトルのサンプル
rs=C/diag(AA)　#r_iを計算
d=AA/c(diag(AA))　#d_{i,j}を計算
diag(d)=0 #d_{i,j}を計算
mat=cbind(rs,-d) #x_iの更新に必要な行列を作成

x=rep(1,ncol(AA))　#x_iの初期値
xpre=rep(5,ncol(AA))
eta=0.01
while(sum(abs(x-xpre))>10^(-5))({
xpre=x  
z=c(1,x)
x=x*(1-eta)+eta*mat%*%z
x=x-mean(x)
print(sum(abs(xpre-x)))

K=-solve(M)%*%(A%*%c(x)+W)
V=-solve(P)%*%t(B)%*%K
#print(2*c(K)%*%(B%*%c(V)+A%*%c(x)+W))

})
X=x
K=-solve(M)%*%(A%*%c(X)+W)
V=-solve(P)%*%t(B)%*%K
cost=c(V)%*%P%*%c(V)-2*c(K)%*%(B%*%c(V)+A%*%c(X)+W)

```

```{r}

#例題8.4
#p.284

A=matrix(c(1,0,0,0,1,0),nrow=3)
B=matrix(c(-1,0,1,0,-1,1,1,1,-1,0,0,-1),nrow=3)
L=c(45,45,15,75)
P=diag(rep(1,4))
x0=20;y0=10
W=c(-L[1]+L[3]+x0,-L[2]+L[3]+y0,L[1]+L[2]-L[3]-L[4])


M=B%*%P%*%t(B)

#条件：x=yを考慮しない
X=-solve(t(A)%*%solve(M)%*%A)%*%t(A)%*%solve(M)%*%W
KL=-solve(M)%*%W-solve(M)%*%A%*%X
V=solve(P)%*%t(B)%*%KL

#条件：x=yを考慮
WC=0
C=c(1,-1)
KC1=solve(C%*%solve(t(A)%*%solve(M)%*%A)%*%C)
KC2=C%*%solve(t(A)%*%solve(M)%*%A)%*%t(A)%*%solve(M)%*%W-WC
KC=KC2/KC1

dx=solve(t(A)%*%solve(M)%*%A)%*%C*as.numeric(KC)
Xhat=X+dx
Vhat=-t(B)%*%solve(M)%*%(A%*%Xhat+W)

```

```{r}

#同一未知量に対し、新・旧の観測が行われた場合の取り扱い
#p.288

A1=matrix(c(1,0,0,0,1,0),nrow=3)
B1=matrix(c(-1,0,1,0,-1,1,1,1,-1,0,0,-1),nrow=3)
A2=matrix(c(1,0,0,0,1,0),nrow=3)
B2=matrix(c(-1,0,1,0,-1,1,1,1,-1,0,0,-1),nrow=3)
L1=c(45,45,15,75)
L2=c(35,50,20,90)
P1=diag(c(1,1,2,4))
P2=diag(c(1,2,4,5))
x10=20;y10=10
x20=11;y20=15
W1=c(-L1[1]+L1[3]+x10,-L1[2]+L1[3]+y10,L1[1]+L1[2]-L1[3]-L1[4])
W2=c(-L2[1]+L2[3]+x20,-L2[2]+L2[3]+y20,L2[1]+L2[2]-L2[3]-L2[4])

M1=B1%*%P1%*%t(B1)
M2=B2%*%P2%*%t(B2)

#条件：x=yを考慮
WC=0
C=c(1,-1)

mat1=cbind(t(A1)%*%M1%*%A1,t(A2),C)
mat2=cbind(A2,-B2%*%solve(P2)%*%t(B2),rep(0,nrow(A2)))
mat3=c(c(C),array(0,dim=c(1,ncol(mat1)-2)))
matrix=rbind(mat1,mat2,mat3)
vec=c(-t(A1)%*%solve(M1)%*%W1,-W2,-WC)


AA=t(matrix)%*%matrix
vector=t(matrix)%*%vec  
x=solve(AA)%*%vector
cbind(matrix%*%x,vec)
X=x[1:nrow(mat1)];x=x[-c(1:nrow(mat1))]
K2=-x[1:nrow(mat2)];x=x[-c(1:nrow(mat2))]
KC=-x
K1=solve(M1)%*%(-W1-A1%*%X)
V1=solve(P1)%*%t(B1)%*%K1
V2=solve(P2)%*%t(B2)%*%K2

```