ROUGH DRAFT authorea.com/113409
Main Data History
Export
Show Index Toggle 0 comments
  •  Quick Edit
  • Симуляция работы фильтра Калмана Петров Евгений (21512)

    Симуляция работы фильтра

    Предположим, что входной сигнал подчиняется следующему уравнению: \[\begin{gathered} \\ x_{t} = \phi \cdot x_{t-1} + u \cdot t + \upsilon_{t} = 0,26 \cdot x_{t-1} + 0,8 \cdot t + \upsilon_{t} , \\ \end{gathered}\] где \(\upsilon_{t} \sim N(0,1)\) произвольная случайная величина. Уравнение наблюдение будет выглядеть следующим образом: \[\begin{gathered} \\ y_{t} = \gamma \cdot x_{t} + \epsilon_{t} = 0,72 \cdot x_{t} + \epsilon_{t} , \\ \end{gathered}\] где \(\epsilon_{t} \sim N(0,1)\) также случайная величина. Более того предполагается, что ошибки наблюдения и сигнала не коррелируемы ( т.е \(E(\epsilon_{t-i} \upsilon_{t-j} , \forall i,j)\) ). По начальной оценке сигнала и дисперсии предсказания, мы можем просимулировать работу данной модели и посчитать результат фильтрации. Сначала необходимо посчитать усиление Калмана и дисперсию предсказания и оценки . Усиление Калмана представляет собой следующее выражение: \[\begin{gathered} \\ K_{N} = \frac{\gamma \cdot S_{N}}{\gamma^2 \cdot S_{N} + \sigma^2_{\epsilon}} \\ \end{gathered}\] где \(S_{N} \) дисперсия ошибки в предсказании \(x_{N}\) в момент \(N-1\), и определяется следующим выражением: \[\begin{gathered} \\ S_{N} = \phi^2 \cdot p^e_{N-1} + \sigma^2_{\upsilon} \\ \end{gathered}\] Дисперсии оценки \(p^e_{N}\) и дисперсия предсказания \(p^p_{N+1}\) равняются: \[\begin{gathered} \\ p^e_{N} = (1 - \gamma \cdot K_{N}) \cdot S_{N} = (1 - 0,72 \cdot K_{N}) \cdot S_{N} \\\\ p^p_{N+1} = p^e_{N} \cdot \phi^2 + \sigma^2_{\upsilon} \\ \end{gathered}\] В начальный момент времени ( \(t = 1 \) ) дисперсия предсказания равна дисперсии сигнала \(p^p_{1} = 0.2 \), \(S_{1} = 1\). Тогда: \[\begin{gathered} \\ \sigma^2_{\upsilon} = 5 \qquad \sigma^2_{\epsilon} = 0,2 \\\\ K_{1} = \frac{0,72 \cdot 1}{0,72^2 \cdot 1 + 0,2} = 1,0022 \\\\ p^e_{1} = (1 - 0,72 \cdot 1,0022) \cdot 1 = 0,2783 \\\\ p^p_{2} = 0,2783 \cdot 0,26^2 + 5 = 5,01881 \\ \end{gathered}\] В момент времени \(t=2\): \[\begin{gathered} \\ K_{2} = \frac{0,72 \cdot 5,01881}{0,72^2 \cdot 5,01881 + 0,2} = 1,2897 \\\\ p^e_{2} = (1 - 0,72 \cdot 1,2897) \cdot 5,01881 = 0,3582 \\\\ p^p_{3} = 0,3582 \cdot 0,26^2 + 5 = 5,0242 \\ \end{gathered}\] В момент времени \(t=3\): \[\begin{gathered} \\ K_{3} = \frac{0,72 \cdot 5,0242}{0,72^2 \cdot 5,0242 + 0,2} = 1,2898 \\\\ p^e_{3} = (1 - 0,72 \cdot 1,2898) \cdot 5,0242 = 0,3582 \\\\ p^p_{4} = 0,3582 \cdot 0,26^2 + 5 = 5,0242 \\ \end{gathered}\] После нескольких шагов, Усиление Калмана, Дисперсия Предсказания и Дисперсия Оценки сводиться к следующим значениям: \[\begin{gathered} \\ \bar k_{mean} = 0,7913 \\ \bar p^p_{mean} = 1,747 \\ \bar p^e_{mean} = 0,2198 \\ \end{gathered}\]

    Предполагаем,что начальная оценка сигнала и предсказанное значение равны нулю. В момент времени \( t = 1\) , после получения первного наблюдения \(y_{1} = -0,3\) первоначальныя оценка сигнала обновляется следующим образом: \[\begin{gathered} \\ \hat{x}^e_{1} = 0 + k_{1} \cdot (y_{1} - 0 ) = 1,0022 \cdot (-0,3 - 0) = -0,30066 \\ \end{gathered}\] На следующем шаге \[\begin{gathered} \\ \hat{x}^p_{2} = 1 + \phi \cdot \hat{x}^e_{1} = 1 + 0,26 \cdot -0,30066 = 0,9218 \\ \end{gathered}\] Предсказанное значение наблюдения \[\begin{gathered} \\ \hat{y}^p_{2} = \gamma \cdot \hat{x}^p_{2} = 0,72 \cdot 0,9218 = 0,6637 \\ \end{gathered}\] В момент времени \(t = 2\), первоначальной оценкой сигнала является предсказание выполненное во время \(t = 1\). Данная оценка обновляется после наблюдения \(y_{2} = 2,127\): \[\begin{gathered} \\ \hat{x}^e_{2} = \hat{x}^p_{2} + k_{2} \cdot (y_{2} - \hat{y}^p_{2} ) = 0,9218 + 1,2897 \cdot (-2,127 - 0,6637) = 2,8090 \\ \end{gathered}\] И \[\begin{gathered} \\ \hat{x}^p_{3} = 1 + \phi \cdot \hat{x}^e_{2} = 1 + 0,26 \cdot 2,8090 = 1,7303 \\\\ \hat{y}^p_{3} = \gamma \cdot \hat{x}^p_{2} = 0,72 \cdot 1,7303 = 1,2458 \\ \end{gathered}\]

    Листинг программы на matlab

    % x(t) = u*t + a*x(t-1) + v_t 
    % y(t) = b*x(t-1) + e_t
    %
    % количество шагов
    Quantity = 20;
    % инициализируем массивы
    k = 1:Quantity;
    % координата
    x = k;
    % предсказанна¤ координата
    x_predic = k;
    % оценочна¤ координата
    x_estim = k;
    % наблюдаема¤ координата
    y = k;
    % предсказанное наблюдение
    y_predic = k;
    % дисперси¤ ошибки в предсказании X в
    Sn = k;
    % усиление калмана
    KalmanGain = k;
    % оценка дисперсии
    EstimVar = k;
    % предсказанна¤ дисперси¤
    PredicVar = k;
    
    %константы
    % коэффииент координаты
    a = 0.26;
    % коэффициент наблюдени¤
    b = 0.72;
    % коэффициент при упрал¤ющей функции
    ControlF = 0.8;
    % дисперси¤ ошибки модели
    disp_ErrorModel = 0.2;
    % дисперис¤ ошибки сенсора
    disp_ErrorSensor = 5;
    % ошибка модели
    ErrorModel = normrnd(0,disp_ErrorModel); 
    % ошибка сенсора
    ErrorSensor = normcdf(0,disp_ErrorSensor); 
    
    
    % заполн¤ем координаты
    x(1) = 0;
    y(1) = b*x(1) +  ErrorSensor;
    for time = 1 : (Quantity-1)
        ErrorModel = normrnd(0,disp_ErrorModel);
        ErrorSensor = normrnd(0,disp_ErrorSensor);
        x(time+1) = a*x(time) +  ControlF*time +