측정값이 거친 초음파 센서의 필터(Prowave 400ST160)

배경.
AR Drone 2.0 컨트롤러를 다시 쓰기 위해 프로세서에 처음부터 쓰기 시작합니다.
MATLAB 공유AR.Drone 2.0 Support from Embedded Coder를 사용합니다.
이미 Parrot사와 뮌헨공과대학(TUM) 등이 공유한 ROS의 포장 아드레날린autoonomy처럼 API를 지능화하려는 것이 아니라 컨트롤러를 좀 더 철저하게 고쳐 쓰려고 매틀랩 포장을 사용했다.
이른바 AR Drone 2.0 Embededed Coder
실제로 AR Drene 2.0에는 BusiBox라는 Linux2.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는 원스톱 타임.
$ 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）

그나저나 오라 코너에서 로테이션 매트릭스로의 전환식은

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