背景

AR Drone 2.0 の制御器を書き換えたいと思いプロセッサーに一から書き込むという作業をしている．
MATLABで共有されている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を積んだプロセッサーが搭載されている．
そこに新しくコードを書き込むことで既に搭載されている制御器を使わないマイ制御器が実装できる．

問題点

超音波センサの値が荒すぎる

やったこと

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

数式

離散カルマンフィルタ

離散の運動方程式

\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 $

観測値

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

加速度

a_{earth}
=
Ra_{body}

ここで$R$はボディー座標系から地球座標系へのRotation Matrix
z軸だけ知りたい&重力加速度は除去したいので

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

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

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のコード

生値からノイズ除去

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;

カルマンフィルタ

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

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

背景