測定値の荒い超音波センサのフィルタリング(Prowave 400ST160)
背景
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}
- 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 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
Author And Source
この問題について(測定値の荒い超音波センサのフィルタリング(Prowave 400ST160)), 我々は、より多くの情報をここで見つけました https://qiita.com/watakandai/items/bd757f2d2aacda7ba91a著者帰属:元の著者の情報は、元のURLに含まれています。著作権は原作者に属する。
Content is automatically searched and collected through network algorithms . If there is a violation . Please contact us . We will adjust (correct author information ,or delete content ) as soon as possible .