測定値の荒い超音波センサのフィルタリング（Prowave 400ST160）

# 背景
AR Drone 2.0 の制御器を書き換えたいと思いプロセッサーに一から書き込むという作業をしている．
MATLABで共有されている[AR.Drone 2.0 Support from Embedded Coder](https://jp.mathworks.com/matlabcentral/fileexchange/48558-ar-drone-2-0-support-from-embedded-coder)を用いる．
既にParrot社やミュンヘン工科大学（TUM)等が共有しているROSのパッケージardrone_autonomyのようにAPIを用いて知能化をしたい訳ではなく， もっと根底の制御器を書き換えたかったのでMATLABのパッケージを用いた．

## AR Drone 2.0 Embedded Coderとは
実はAR Drone 2.0はBusyBoxというLinux 2.6.32を積んだプロセッサーが搭載されている．
そこに新しくコードを書き込むことで既に搭載されている制御器を使わないマイ制御器が実装できる．

# 問題点
**超音波センサの値が荒すぎる**
![image](https://user-images.githubusercontent.com/11141442/38612363-60d64360-3dc1-11e8-8a01-18fa30a3df3a.png)


# やったこと
1. 生データから現実的な値のみ選択  
 - 10000以上の値は捨てる
 - 1ステップ前(400Hz)のデータとの差分が600以内の値は選択）
2. カルマンフィルタをかける
 - **加速度センサの情報を用いる**

## 数式
### 離散カルマンフィルタ
#### 離散の運動方程式
```math
\begin{bmatrix}
x_t \\ v_t \\ a_t
\end{bmatrix}
=
\begin{bmatrix}
1 & Ts & 0 \\
0 & 1 & Ts \\
0 & 0 & 1 
\end{bmatrix}
\begin{bmatrix}
x_{t-1} \\ v_{t-1} \\ a_{t-1}
\end{bmatrix}
```
ここでTsは1ステップ時間s 
$ Ts = 1/400 $

#### 観測値

```math
\begin{bmatrix}
位置 \\ 加速度
\end{bmatrix}
=
\begin{bmatrix}
x_{t-1} \\ a_{t-1}
\end{bmatrix}
```

#### 加速度
```math
a_{earth}
=
Ra_{body}
```
ここで$R$はボディー座標系から地球座標系へのRotation Matrix
z軸だけ知りたい&重力加速度は除去したいので

```math
a_{actual} = 
\begin{bmatrix}
0 & 0 & 1
\end{bmatrix}
a_{earth} - g （もしくは+g）
```

ちなみにオイラー角からRotation Matrixへの変換式は

```matlab
function R = euler2rotMat(euler_angle)
R=eye(3);
phi = euler_angle(1);
theta = euler_angle(2);
psi = euler_angle(3);
    R(1,1) = cos(psi).*cos(theta);
    R(1,2) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi);
    R(1,3) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi);
 
    R(2,1) = sin(psi).*cos(theta);
    R(2,2) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi);
    R(2,3) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi);
 
    R(3,1) = -sin(theta);
    R(3,2) = cos(theta).*sin(phi);
    R(3,3) = cos(theta).*cos(phi);
end
```


# Simulink Blockのコード
## 生値からノイズ除去
```matlab
function UltraSound_Calculation = removePeak(Ultrasound_Measure) 
peakThreshold = 10000;
 
% initialization
persistent validHeight
if isempty(validHeight)
    validHeight = 0;
end

% only use the valid data
if Ultrasound_Measure < peakThreshold
    if validHeight~=0 && abs(validHeight-Ultrasound_Measure)<600
        validHeight = Ultrasound_Measure;
    elseif validHeight==0
        validHeight = Ultrasound_Measure;
    end
end

% Convert Raw Sensor Value to Actual Measurement in Meter
UltraSound_Calculation = validHeight*(2.8*10^-4) + 2.6*10^-2;

```
![image.png](https://qiita-image-store.s3.amazonaws.com/0/146448/ed4f59ee-bf00-67c0-1ab5-2451115dbc4b.png)

## カルマンフィルタ
```matlab
function estHeight = fcn(UltraSoundMeasure, Rot, accBody, m1, m2)

%-------------------------------
%-------------------------------
%---- Predict Stage ------------
%-------------------------------
%-------------------------------
dim=3;
persistent Xhat
if isempty(Xhat)
    Xhat = [0 0 0]';
end
persistent P
if isempty(P)
    P=eye(dim);
end
persistent n
if isempty(n)
    n=0;
end
 
Ts = 1/400;
 
%Process Noise
p=0.1;
Q = [p^2 0 0; 
     0 p^2 0;
     0 0 p^2];
 
%A matrix
A = [1 Ts 0;
     0 1 Ts; 
     0 0 1];
 
%odutput these values with a unit delay (for algebraic loop), apply initial conditions
Xhatm = A*Xhat;
Pm = A*P*A' + Q;
 
%-------------------------------
%-------------------------------
%---- Estimate Stage -----------
%-------------------------------
%-------------------------------
    %C matrix
    C=[1 0 0; 0 0 1];
 
    %Measurement noise
    R = diag([m1^2 m2^2]);

    %Measurement Z
    n=n+1;
    accEarth = -([0 0 1]*Rot*accBody + 1);
    if abs(Xhat(1) - UltraSoundMeasure) < 0.3
      Y = [UltraSoundMeasure accEarth]';
      n=0;
    else
      Y = [Xhat(1) accEarth]';
   end
  
    %Compute Kalman Gain
    Kalman_Gain = Pm*C'*inv(C*Pm*C'+R);
 
    %compute state space correction based on measurement
    Xhat = Xhatm + Kalman_Gain*(Y - C*Xhatm);
 
    %compute covariance correction
    P = (eye(dim) - Kalman_Gain*C)*Pm;
 
   estHeight = Xhat;
end

```
![image.png](https://qiita-image-store.s3.amazonaws.com/0/146448/7f69e45b-62d2-779b-7b19-5796bf70e857.png)