# 2022軟體組交接

## System

### STM32

### Rpi

## Sensor

### IMU

### Pressure Sensor

## Sonar

## Output

### Motor

## Control

### Dynamics

#### Full Dynamic Model

- 座標圖示:

- Dynamic Equation

$$
\dot{x}=v \\
m\ddot{x}=mge_3+TR-Be_3-K_v(R)v^2-F_L(R) \\
\dot{R}=R\hat{\Omega} \\
J\hat{\Omega}+\Omega\times J\Omega=M-K_{\Omega}(R){\Omega}^2+M_B(R)+M_f(v)
$$

- x: 3D vector, position of AUV
- v: 3D vector, velocity of AUV
- m: scalar, mass of AUV
- g: scalar, gravity acceleration constant
- e~3~: 3D vector, [0 0 1]^T^
- T: 3D vector, motor force
- R: 9*9 matrix, rotation matrix of AUV
- B: scalar, value of buoyancy
- K~v~\(R\): drag constant, caused by the cross section of AUV --> a function of AUV attitude
- F~L~\(R\): lift, caused by the shape of AUV --> a function of AUV attitude
- Ω: angular velocity of AUV
- J: moment of inertia of AUV
- M: motor moment
- K~Ω~\(R\): drag moment constant, a function of AUV attitude
- M~B~\(R\): moment by buoyancy
- M~f~: moment by drag force

#### Simplified Dynamic Model

- Dynamic Equation

$$
\dot{x}=v \\
m\ddot{x}=mge_3+TR-Be_3-K_vv^2 \\
\dot{R}=R\hat{\Omega} \\
J\hat{\Omega}=M+M_B(R)+M_f(v)
$$

- Current Equation

$$
\dot{x}=v \\
m\ddot{x}=mge_3+TR-Be_3 \\
\dot{R}=R\hat{\Omega} \\
J\hat{\Omega}=M
$$

### Error Term

#### Linear

1. **e~x~:** 由兩個1D聲納，測量牆壁計算得到位置，再將前後時刻的位置相減


2. **e~v~:** 由DVL計算+聲納計算出的位移微分，兩者融合而成

#### Angular

1. **Roll & Pitch**

由IMU得到，透過e~R~的公式計算得到，公式如下:

$$
e_R=\frac{1}{2}(R_d^TR-R^TR_d)^\bigvee
$$

2. **Yaw**


透過兩個聲納，從上圖的三角關係中，$\theta$ 角已知，$l_1$、$l_2$由聲納得到，可透過餘弦定理算出$l_3$及 $\alpha$ 角

### Control Input