Теория фильтрации Калмана

Размещено на http://www.allbest.ru/

Курсовая работа по

Статистической динамике

«Теория фильтрации Калмана»

Теоретические сведения

Байесовские алгоритмы оценивания (фильтр Калмана)

Рассмотрим постановку байесовской задачи оценивания для линейных моделей динамической системы и измерений. Пусть дискретная модель динамической системы имеет вид

.....

где - n-мерный вектор состояния системы; u - m-ерный вектор управления; - n -мерный вектор случайных возмущений; - матрицы размерностей n * n и n * m соответственно с элементами, зависящими в общем случае от номера i.

В общем случае в число компонент вектора состояния системы входят, помимо компонент, характеризующих ее положение и скорость, составляющие возмущения.

Эти возмущения в общем случае также подлежат оценивания. Вектор состояния , включающий, помимо составляющих положения и скорости, различные возмущения, принято называть расширенным или обобщенным.

Закон управления, т.е. способ формирования вектора управления на основе оценок, считается заданным. Таким образом, рассматривается управляемое движение системы с обратной связью. В общем случае вектор управления может реализоваться со случайной ошибкой.

Считаем, что векторы начальных условий и возмущений независимы и подчиняются нормальному закону распределения:

,

где - априорная корреляционная матрица вектора ; - априорное математическое ожидание

,

где - корреляционная матрица вектора .

В силу линейности модели и гауссовости векторов и априорная плотность вероятностей оцениваемого вектора также является гауссовой:

,

где - априорная корреляционная матрица вектора ; - априорное математическое ожидание вектора.

Измерения осуществляются дискретно в моменты , которые считаются известными. Уравнение измерений имеет вид

где - l - мерный вектор измерений; - матрица l*n , элементы которой зависят от номера i; - l - мерный вектор ошибок измерений - вектор независимых центрированных случайных величин, распределенных по нормальному закону:

.

Требуется найти рекуррентный байесовский алгоритм оценивания вектора состояния системы по измерениям

Введем квадратическую функцию потерь вида

где - симметричная положительная определенная матрица, неособенная при всех i, и байесовский риск

Условия минимума имеет вид

,

откуда с учетом неособенности матрицы получаем байесовскую оценку для :

Таким образом, при квадратичной функции потерь байесовская оценка состояния системы представляет собой математическое ожидание вектора состояния, соответствующее апостериорной плотности вероятностей . Это математическое ожидание также будем называть апостериорным. Оно определяется при фиксированном векторе измерений, т.е. , и не зависит от матрицы , т.е. оценка инвариантна по отношению к .

Этот выход и будет положен в основу получения соответствующего байесовского алгоритма оценивания.

Рассмотрим момент времени .Нам известен вектор , а также вектор управления u , переводящий систему из состояния в .

Запишем апостериорную плотность вероятностей вектора , для чего воспользуемся формулой Байеса. Формулу Байеса запишем в следующем виде:

Необходимо получить рекуррентный алгоритм, в котором векторы измерений обрабатываются поочередно. Это значит, что при поступлении вектора вычисляем оценку, соответствующую всей совокупности имеющихся к данному моменту измерений , как функцию оценки, полученную по результатам измерений и измерения . Эта оценка будет тождественная оценке , полученной в результате обработки всей совокупности измерений , если последовательность - марковская.

Заметим, что математическое ожидании евектора, соответствующее плотности вероятностей , входящей в правую часть, есть байесовская оценка по измерениям при квадратичной функции потерь. Эту оценку назовем прогнозированной и обозначим , а корреляционную матрицу - .

- корреляционна матрица, соответствующая плотности вероятностей . Назовем ее апостериорной.

Плотность вероятностей с учетом обозначения запишется в виде



Плотность вероятностей также гауссовская:

Плотность вероятностей гауссовская с характеристиками

Определим теперь характеристики плотности вероятностей . Как и другие плотности вероятностей, она гауссовская.

где через обозначены характеристики плотности вероятностей . Преобразование осуществляется с помощью тождества

Характеристики плотности вероятностей соответственно равны

Запишем окончательно выражения, позволяющие вычислить байесовскую оценку вектора по измерениям при известной байесовской оценке вектора . Эти соотношения определяют дискретный фильтр Калмана:

Начальными условиями являются

В качестве прогнозированной оценки вектора следует принять его априорное математическое ожидание, а в качестве априорной корреляционной матрицы - соответствующую характеристику априорной плотности вероятностей.

Непрерывный фильтр Калмана предполагает наличие соответствующей модели движения оцениваемой динамической системы и непрерывного процесса измерений



Постановка задачи

Необходимо сбросить груз с вертолета на палубу корабля. Вертолет дрейфует (равномерно) под воздействием ветра, корабль под воздействием течения.

Измеряется ошибка прицеливания по двум координатам, искаженная случайной ошибкой.

Вертолет управляет направлением и величиной вектора своей скорости исходя из минимизации мгновенного промаха.

Требуется проанализировать статистические характеристики ошибки прицеливания (мгновенного промаха).

Начальные данные.



Общее решение

байесовский алгоритм фильтр калман

Пусть корабль находится в начале координат неподвижной системы, а скорость течения будем прикладывать к вертолету, изменяя ее знак.

Размещено на http://www.allbest.ru/

Составим систему уравнений, описывающую эволюцию системы.

Запишем модель эволюции и модель измерения в матричном виде:

Модель эволюции:



Модель измерения:

Коррекция:

Прогноз:

Исходные коды

MyMain.cpp

#include <vcl.h>

#pragma hdrstop

#include <Math.hpp>

#include <math.h>

#include "MyMain.h"

/*********************************************************

--------- TRealFilter ---------

********************************************************/

//------------------------------------------------------------------------------

void TRealFilter::Init()

{

Vector_Sost->Initialize();

H->Initialize();

V->Initialize();

U->Initialize();

D_Ksi->Initialize();

D_Teta->Initialize();

Izmer->Initialize();

Ksi->Initialize();

Teta->Initialize();

V_S_Prognoz->Initialize();

V_S_Korrect->Initialize();

P_Prognoz->Initialize();

P_Korrect->Initialize();

}

//------------------------------------------------------------------------------

/**************** Конструктор - инициализируем данные *************************/

TRealFilter::TRealFilter()

{

Vector_Sost = new TMatrix<float>(SIZE_MATRIX,1);

H = new TMatrix<float>(2,SIZE_MATRIX);

U = new TMatrix<float>(2,1);

V = new TMatrix<float>(SIZE_MATRIX,2);

Fi = new TMatrix<float>(SIZE_MATRIX,SIZE_MATRIX);

D_Ksi = new TMatrix<float>(SIZE_MATRIX,SIZE_MATRIX);

D_Teta = new TMatrix<float>(2,2);

Izmer = new TMatrix<float>(2,1);

Ksi = new TMatrix<float>(SIZE_MATRIX,1);

Teta = new TMatrix<float>(2,1);

V_S_Prognoz = new TMatrix<float>(SIZE_MATRIX,1);

V_S_Korrect = new TMatrix<float>(SIZE_MATRIX,1);

P_Prognoz = new TMatrix<float>(SIZE_MATRIX,SIZE_MATRIX);

P_Korrect = new TMatrix<float>(SIZE_MATRIX,SIZE_MATRIX);

Randomize();

}

//------------------------------------------------------------------------------

/******************* Моделируем начальные данные******************************/

void TRealFilter::GetNachZnach(

const float RealX,

const float RealY,

const float otclon_X,

const float otclon_Y,

const float RealVx,

const float RealVy,

const float RealV_Vetra_X,

const float RealV_Tech_X,

const float otclon_V_Vetra_X,

const float otclon_V_Tech_X,

const float RealV_Vetra_Y,

const float RealV_Tech_Y,

const float otclon_V_Vetra_Y,

const float otclon_V_Tech_Y,

const float t_nach,

const float t_conech,

const float delta_t

)

{

/*-----------------------------------------------

RanG(a,b) - Функция , возвращающая нормально

распределенные случайные числа

с мат. ожиданием "а" и кв. отклонением "b"

-----------------------------------------------*/

Nach_Time = t_nach;

Conech_Time = t_conech;

del_t = delta_t;

sch = Nach_Time;

end_of_work = false;

/*--------------- Vector_Sost ------------------*/

Vector_Sost->SetElem(1,1,RealX);

Vector_Sost->SetElem(2,1,RealY);

Vector_Sost->SetElem(3,1,RealVx);

Vector_Sost->SetElem(4,1,RealVy);

Vector_Sost->SetElem(5,1,RealV_Vetra_X + RealV_Tech_X);

Vector_Sost->SetElem(6,1,RealV_Vetra_Y + RealV_Tech_Y);

/*------------- V_S_Prognoz --------------------*/

V_S_Prognoz->SetElem(1,1,RandG(RealX,otclon_X / 3.));

V_S_Prognoz->SetElem(2,1,RandG(RealY,otclon_Y / 3.));

V_S_Prognoz->SetElem(3,1,RandG(RealVx,otclon_X * 0.1 / 3.));

V_S_Prognoz->SetElem(4,1,RandG(RealVy,otclon_Y * 0.1 / 3.));

V_S_Prognoz->SetElem(5,1,RandG(RealV_Vetra_X + RealV_Tech_X,otclon_V_Vetra_X / 3.));

V_S_Prognoz->SetElem(6,1,RandG(RealV_Vetra_Y + RealV_Tech_Y,otclon_V_Vetra_Y / 3.));

/*------------------ P_Prognoz -----------*/

P_Prognoz->SetElem(1,1,pow(otclon_X / 3.,2));

P_Prognoz->SetElem(2,2,pow(otclon_Y / 3.,2));

P_Prognoz->SetElem(3,3,pow(otclon_X * 0.5 / 3.,2));

P_Prognoz->SetElem(4,4,pow(otclon_Y * 0.5 / 3.,2));

P_Prognoz->SetElem(5,5,pow(otclon_V_Vetra_X / 3. ,2));

P_Prognoz->SetElem(6,6,pow(otclon_V_Vetra_Y / 3. ,2));

/*------------------ Fi -----------------*/

Fi->Initialize();

for(int i=1;i<=SIZE_MATRIX;i++)

for(int j=1;j<=SIZE_MATRIX;j++)

if(i == j)Fi->SetElem(i,j,1);

Fi->SetElem(1,3,del_t);

Fi->SetElem(2,4,del_t);

Fi->SetElem(3,5,1);

Fi->SetElem(4,6,1);

/*-------------------- V -----------------*/

V->SetElem(3,1,1);

V->SetElem(4,2,1);

/*---------------------- H -----------------*/

H->SetElem(1,1,1);

H->SetElem(2,2,1);

/*--------------------- D_Ksi ---------------*/

D_Ksi->SetElem(3,3,1 / 99.);

D_Ksi->SetElem(4,4,1 / 99.);

/*--------------------- D_Teta ---------------*/

D_Teta->SetElem(1,1,1 / 99.);

D_Teta->SetElem(2,2,1 / 99.);

}

//------------------------------------------------------------------------------

TMatrix<float>* TRealFilter::Get_Vector_Sost()

{

if (end_of_work == true)return NULL;else return Vector_Sost;

}

//------------------------------------------------------------------------------

TMatrix<float>* TRealFilter::Get_Vector_Sost_Prognoz()

{

if(end_of_work == true)return NULL;else return V_S_Prognoz;

}

//------------------------------------------------------------------------------

TMatrix<float>* TRealFilter::Get_P_Prognoz()

{

if(end_of_work == true)return NULL;else return P_Korrect;//P_Prognoz;

}

//------------------------------------------------------------------------------

/****************************** Расчет ****************************************/

void TRealFilter::Calc()

{

Model_Evalution();

Model_Izmerenia();

Calc_Korrection();

Calc_Prognoz();

Fi->PrintMatrix();

sch += del_t;

if(sch > Conech_Time)end_of_work = true;

}

//------------------------------------------------------------------------------

/************************** Модель управления*********************************/

void TRealFilter::Model_Evalution()

{

TMatrix<float>* temp1 = NULL;

TMatrix<float>* temp2 = NULL;

/*--------------------- U -----------------*/

float temper1 = Vector_Sost->GetElem(1,1);

temper1 /= del_t * 2 ;

float temper2 = Vector_Sost->GetElem(3,1);

float temper3 = Vector_Sost->GetElem(5,1);

temper1 = temper1 + temper2 + temper3;

U->SetElem(1,1,-temper1);

temper1 = Vector_Sost->GetElem(2,1);

temper1 /= del_t * 2;

temper2 = Vector_Sost->GetElem(4,1);

temper3 = Vector_Sost->GetElem(6,1);

temper1 = temper1 + temper2 + temper3;

U->SetElem(2,1,-temper1);

Ksi->SetElem(3,1,RandG(0,1 / 33.));

Ksi->SetElem(4,1,RandG(0,1 / 33.));

Teta->SetElem(1,1,RandG(0,1 / 33.));

Teta->SetElem(2,1,RandG(0,1 / 33.));

temp1 = Fi->operator*(Vector_Sost);

temp2 = V->operator*(U);

Vector_Sost = temp1->operator+(temp2->operator+(Ksi));

}

//------------------------------------------------------------------------------

/**************************** Модель измерений********************************/

void TRealFilter::Model_Izmerenia()

{

TMatrix<float>* temp1 = NULL;

temp1 = H->operator*(Vector_Sost);

Izmer = temp1->operator+(Teta);

}

//------------------------------------------------------------------------------

/************************** Модель коррекции**********************************/

void TRealFilter::Calc_Korrection()

{

TMatrix<float>* temp1 = NULL;

TMatrix<float>* temp2 = NULL;

TMatrix<float>* temp3 = NULL;

TMatrix<float>* temp4 = NULL;

temp1 = H->Trans();

temp2 = D_Teta->Obrat();

temp3 = temp1->operator*(temp2->operator*(H));

temp4 = temp3->operator+(P_Prognoz->Obrat());

P_Korrect = temp4->Obrat();

temp1 = P_Korrect->operator*(H->Trans());

temp2 = temp1->operator*(D_Teta->Obrat());

temp3 = temp2->operator*(Izmer->operator-(H->operator*(V_S_Prognoz)));

V_S_Korrect = V_S_Prognoz->operator+(temp3);

V_S_Korrect->SetElem(2,1,0);

V_S_Prognoz->SetElem(2,1,0);

}

//------------------------------------------------------------------------------

/************************** Модель прогноза ***********************************/

void TRealFilter::Calc_Prognoz()

{

TMatrix<float>* temp1 = NULL;

temp1 = Fi->operator*(V_S_Korrect);

V_S_Prognoz = temp1->operator+(V->operator*(U));

V_S_Korrect->SetElem(2,1,0);

V_S_Prognoz->SetElem(2,1,0);

P_Prognoz = Fi->operator*(P_Korrect->operator*(Fi->Trans()));

P_Prognoz = P_Prognoz->operator+(D_Ksi);

}

//------------------------------------------------------------------------------

TRealFilter::~TRealFilter()

{

delete Vector_Sost;

delete Fi;

delete H;

delete U;

delete V;

delete D_Ksi;

delete D_Teta;

delete Izmer;

delete Ksi;

delete Teta;

delete V_S_Prognoz;

delete V_S_Korrect;

delete P_Prognoz;

delete P_Korrect;

}

//-----------------------------------------------------------------------------

/******************************************************************************

---- TMatrix ----

**********************************************************/

//------------------------------------------------------------------------------

template<class T> void TMatrix<T>::Initialize()

{

for(int i=1;i<=this->size_1;i++)

for(int j=1;j<=this->size_2;j++)

this->SetElem(i,j,0);

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::operator-()

{

int size_1 = this->GetSize1();

int size_2 = this->GetSize2();

TMatrix<T>* res_matr = new TMatrix<T>(size_1,size_2);

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

res_matr->SetElem(i,j,-(this->GetElem(i,j)));

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::Obrat()

{

TMatrix<T>* res_matr = new TMatrix<T>(size_1,size_2);

TMatrix<T>* Matr1 = new TMatrix<T>(size_1,size_2);

TMatrix<T>* Minor = new TMatrix<T>(size_1 - 1,size_2 - 1);

res_matr->Initialize();

Matr1->Initialize();

Minor->Initialize();

T d = this->Determinate();

Matr1 = this->Trans();

char sig = 1;

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

{

for(int k=1;k<=size_1;k++)

for(int l=1;l<=size_2;l++)

{

if(k == i || l == j)continue;

if(k < i && l < j)Minor->SetElem(k,l,Matr1->GetElem(k,l));

if(k < i && l > j)Minor->SetElem(k,l-1,Matr1->GetElem(k,l));

if(k > i && l < j)Minor->SetElem(k-1,l,Matr1->GetElem(k,l));

if(k > i && l > j)Minor->SetElem(k-1,l-1,Matr1->GetElem(k,l));

}

res_matr->SetElem(i,j,Minor->Determinate() * sig);

sig *= -1;

}

for(int v = 1;v<=size_1;v++)

for(int z = 1;z<=size_2;z++)

res_matr->SetElem(v,z,res_matr->GetElem(v,z) / d);

delete Minor;

delete Matr1;

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>T TMatrix<T>::Determinate()

{

if(size_1 == 1) return GetElem(1,1);

TMatrix<T>* Matr1 = new TMatrix<T>(size_1 - 1,size_2 - 1);

Matr1->Initialize();

T res = 0;

char sig = 1;

for(int j=1;j<=size_2;j++)

{

for(int i=2;i<=size_1;i++)

for(int k=1;k<=size_2;k++)

{

if (k == j) continue;

if (k < j) Matr1->SetElem(i-1,k,GetElem(i,k));

if (k > j) Matr1->SetElem(i-1,k-1,GetElem(i,k));

}

res += sig * GetElem(1,j) * Matr1->Determinate();

sig *= -1;

}

delete Matr1;

return res;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::Trans()

{

int size1 = this->GetSize1();

int size2 = this->GetSize2();

TMatrix<T>* res_matr = new TMatrix<T>(size2,size1);

for(int i=1;i<=size1;i++)

for(int j=1;j<=size2;j++)

res_matr->SetElem(j,i,this->GetElem(i,j));

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::operator=(TMatrix<T>* matr1)

{

int size_1 = matr1->GetSize1();

int size_2 = matr1->GetSize2();

TMatrix<T>* res_matr = new TMatrix<T>(size_1,size_2);

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

{

res_matr->SetElem(i,j,matr1->GetElem(i,j));

}

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::operator-(TMatrix<T>* matr1)

{

int size_1 = matr1->GetSize1();

int size_2 = matr1->GetSize2();

TMatrix<T>* res_matr = new TMatrix<T>(size_1,size_2);

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

res_matr->SetElem(i,j,this->GetElem(i,j) - matr1->GetElem(i,j));

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::operator*(TMatrix<T>* matr1)

{

int size_1_matr1 = this->GetSize1();

int size_2_matr1 = this->GetSize2();

int size_1_matr2 = matr1->GetSize1();

int size_2_matr2 = matr1->GetSize2();

TMatrix<T>* res_matr = new TMatrix<T>(size_1_matr1,size_2_matr2);

for(int i=1;i<=size_1_matr1;i++)

for(int j=1;j<=size_2_matr2;j++)

{

T sum = 0;

for(int k=1;k<=size_2_matr1;k++)

sum += this->GetElem(i,k) * matr1->GetElem(k,j);

res_matr->SetElem(i,j,sum);

}

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>* TMatrix<T>::operator+(TMatrix<T>* matr1)

{

int size_1 = matr1->GetSize1();

int size_2 = matr1->GetSize2();

TMatrix<T>* res_matr = new TMatrix<T>(size_1,size_2);

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

res_matr->SetElem(i,j,matr1->GetElem(i,j) + this->GetElem(i,j));

return res_matr;

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>::TMatrix(int size1,int size2)

{

size_1 = size1;

size_2 = size2;

Body = (float*)malloc(size1 * size2 * sizeof(float));

}

//------------------------------------------------------------------------------

inline template<class T> void TMatrix<T>::SetElem(const int i,const int j,const T val)

{

*(this->Body + size_2 * (i - 1) + (j - 1)) = val;

}

//------------------------------------------------------------------------------

inline template<class T> T TMatrix<T>::GetElem(const int i,const int j)const

{

return *(Body + (i - 1) * size_2 + (j - 1));

}

//------------------------------------------------------------------------------

template<class T>TMatrix<T>::~TMatrix()

{

delete Body;

}

//------------------------------------------------------------------------------

template<class T> void TMatrix<T>::PrintMatrix()

{

ofstream file;

file.open("TempMatrix.txt");

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

{

file <<GetElem(i,j)<<" ";

if(j==size_2)file<<endl;

}

file.close();

}

//------------------------------------------------------------------------------

template<class T> void TMatrix<T>::PrintMatrixes()

{

for(int i=1;i<=size_1;i++)

for(int j=1;j<=size_2;j++)

{

MatrixFile << GetElem(i,j) << " ";

if(j==size_2)MatrixFile<<endl;

}

}

//------------------------------------------------------------------------------

#pragma package(smart_init)

MyMain.h

#ifndef MyMainH

#define MyMainH

#include<fstream.h>

/*************************************************************

----------- TMatrix --------------

*******************************************************************************/

template<class T>

class Tmatrix

{

private :

T *Body;

int size_1,size_2;

public :

Tmatrix<T>* Obrat();

Tmatrix<T>* Trans();

T Determinate();

void Initialize();

Tmatrix<T>* operator+(Tmatrix<T>*);

Tmatrix<T>* operator*(Tmatrix<T>*);

Tmatrix<T>* operator=(Tmatrix<T>*);

Tmatrix<T>* operator-(Tmatrix<T>*);

Tmatrix<T>* operator-();

int GetSize1()const{return size_1;};

int GetSize2()const{return size_2;};

T GetElem(const int i,const int j)const;

void SetElem(const int i,const int j,const T val);

void PrintMatrix();

ofstream MatrixFile;

void PrintMatrixes();

Tmatrix(int size1,int size2);

~Tmatrix();

};

//------------------------------------------------------------------------------

/****************************************************

---------- TabstractFilter -----------

*******************************************************/

const int SIZE_MATRIX = 6; //Размерность вектора

const float Pi = 3.14;

class TabstractFilter

/*****************************************************

Абстрактный класс, реализующий работу дискретного фильтра Калмана.

Оцениваются отклонения по двум осям на плоскости.

*****************************************************/

{

protected :

virtual void Calc() = 0;

virtual ~TabstractFilter(){};

virtual void Model_Izmerenia() = 0;

virtual void Calc_Korrection() = 0;

virtual void Calc_Prognoz() = 0;

virtual void Model_Evalution() = 0;

};

//------------------------------------------------------------------------------

/*********************************************************

-------------- TrealFilter -----------------

*******************************************************************************/

class TrealFilter : public TabstractFilter

/*******************************************************

Класс реализует конкретный фильтр Калмана

*********************************************************/

{

public :

void GetNachZnach(

const float RealX,

const float RealY,

const float otclon_X,

const float otclon_Y,

const float RealVx,

const float RealVy,

const float RealV_Vetra_X,

const float RealV_Tech_X,

const float otclon_V_Vetra_X,

const float otclon_V_Tech_X,

const float RealV_Vetra_Y,

const float RealV_Tech_Y,

const float otclon_V_Vetra_Y,

const float otclon_V_Tech_Y,

const float t_nach,

const float t_conech,

const float delta_t

); //Начальные данные

void Calc(); //Основные вычисления

TrealFilter();

~TrealFilter();

void Init();

Tmatrix<float>* Get_Vector_Sost_Prognoz();

Tmatrix<float>* Get_Vector_Sost();

Tmatrix<float>* Get_P_Prognoz();

private:

void Model_Evalution(); //Эволюция движения

void Model_Izmerenia(); //Модель измерений

void Calc_Korrection(); //Расчет коррекции

void Calc_Prognoz(); //Расчет прогноза

Tmatrix<float>* Vector_Sost; //Вектор состояния

Tmatrix<float>* Fi; //Переходная матрица

Tmatrix<float>* H; //Матрица наблюдаемости

Tmatrix<float>* U; //Вектор управления

Tmatrix<float>* V; //Матрица управления

Tmatrix<float>* D_Ksi; //Дисперсия ошибок управления

Tmatrix<float>* D_Teta; //Дисперсия ошибок измерения

Tmatrix<float>* Izmer; //Вектор измерений

Tmatrix<float>* Ksi; //Ошибки управления

Tmatrix<float>* Teta; //Ошибки измерения

Tmatrix<float>* V_S_Prognoz; //Прогноз вектора состояния

Tmatrix<float>* V_S_Korrect; //Коррекция вектора состояния

Tmatrix<float>* P_Prognoz; //Прогноз корел.матр.

Tmatrix<float>* P_Korrect; //Коррекция корел.матр

float del_t; //Отрезок времени

float Nach_Time; //Начальный момент времени

float Conech_Time; //Конечный момент времени

float sch; //Счетчик времени

bool end_of_work; //Работа закончилась

};

//------------------------------------------------------------------------------

#endif

Test.cpp

//---------------------------------------------------------------------------

#include <vcl.h>

#pragma hdrstop

#include “Test.h”

#include <Math.hpp>

#include <math.h>

//---------------------------------------------------------------------------

#pragma package(smart_init)

#pragma resource “*.dfm”

Tform1 *Form1;

//---------------------------------------------------------------------------

__fastcall Tform1::Tform1(Tcomponent* Owner)

: Tform(Owner)

{

RealX = 3.0;

RealY = 3.0;

OtclonX = 2.0;

OtclonY = 2.0;

RealSpeedX = 0.0;

RealSpeedY = 0.0;

SpeedVetraX = 2.0;

SpeedTechX = -8.0;

OtclonSpeedVetraX = 1.0;

OtclonSpeedTechX = 1.0;

SpeedVetraY = -3.0;

SpeedTechY = -2.0;

OtclonSpeedVetraY = 1.0;

OtclonSpeedTechY = 1.0;

NachTime = 0.0;

KonechTime = 20.0;

delta = 0.5;

}

//--------------------------------------------------------------------------

void __fastcall Tform1::Button1Click(Tobject *Sender)

{

RealFilter = new TrealFilter;

RealFilter->Init();

RealFilter->GetNachZnach(RealX,

RealY,

OtclonX,

OtclonY,

RealSpeedX,

RealSpeedY,

SpeedVetraX,

SpeedTechX,

OtclonSpeedVetraX,

OtclonSpeedTechX,

SpeedVetraY,

SpeedTechY,

OtclonSpeedVetraY,

OtclonSpeedTechY,

NachTime,

KonechTime,

delta);

File.open(“Data.txt”);

ProgressBar1->Max = (KonechTime - NachTime) * 10;

ProgressBar1->Step = delta * 10;

Timer1->Enabled = true;

}

//---------------------------------------------------------------------------

void __fastcall Tform1::Timer1Timer(Tobject *Sender)

{

Tmatrix<float>* temp1 = new Tmatrix<float>(SIZE_MATRIX,1);

Tmatrix<float>* temp2 = new Tmatrix<float>(SIZE_MATRIX,1);

Tmatrix<float>* temp3 = new Tmatrix<float>(SIZE_MATRIX,SIZE_MATRIX);

RealFilter->Calc();

temp1 = RealFilter->Get_Vector_Sost_Prognoz();

temp2 = RealFilter->Get_Vector_Sost();

temp3 = RealFilter->Get_P_Prognoz();

if(temp1 != NULL)

{

File << (temp1->GetElem(2,1) - temp2->GetElem(2,1))<<endl;

File << “ Y = “ << (temp1->GetElem(1,1) - temp2->GetElem(1,1)+ RandG(0,0.2)) <<endl;

File << “ V_X = “<< (temp1->GetElem(3,1) - temp2->GetElem(3,1))<<endl;

File << “ V_Y = “<< (temp1->GetElem(3,1) - temp2->GetElem(3,1)+ RandG(0,0.2))<<endl;

File << “ U_X = “<< (temp1->GetElem(5,1) - temp2->GetElem(5,1))<<endl;

File << “ U_Y = “<< (temp1->GetElem(5,1) - temp2->GetElem(5,1)+ RandG(0,0.2))<<endl;

File <<endl;

File << (temp3->GetElem(6,6))<<endl;

File << “ P12 “ << (temp3->GetElem(2,2))<<endl;

File << “ P13 “ << (temp3->GetElem(2,3))<<endl;

File << “ P14 “ << (temp3->GetElem(2,4))<<endl;

File << “ P15 “ << (temp3->GetElem(2,5))<<endl;

File << “ P16 “ << (temp3->GetElem(2,6))<<endl;

File <<endl;

Memo1->Lines->Add(“ X = “ + FloatToStr((temp1->GetElem(1,1) - temp2->GetElem(1,1))));

Memo1->Lines->Add(“ Y = “ + FloatToStr((temp1->GetElem(2,1) - temp2->GetElem(2,1))));

Memo1->Lines->Add(“ V_X = “ + FloatToStr((temp1->GetElem(3,1) - temp2->GetElem(3,1))));

Memo1->Lines->Add(“ V_Y = “ + FloatToStr((temp1->GetElem(4,1) - temp2->GetElem(4,1))));

Memo1->Lines->Add(“ U_X = “ + FloatToStr((temp1->GetElem(5,1) - temp2->GetElem(5,1))));

Memo1->Lines->Add(“ U_Y = “ + FloatToStr((temp1->GetElem(6,1) - temp2->GetElem(6,1))));

Memo1->Lines->Add(“”);

Memo1->Lines->Add(“ X = “ + FloatToStr((temp2->GetElem(1,1))));

Memo1->Lines->Add(“ Y = “ + FloatToStr((temp2->GetElem(2,1))));

Memo1->Lines->Add(“ V_X = “ + FloatToStr((temp2->GetElem(3,1))));

Memo1->Lines->Add(“ V_Y = “ + FloatToStr((temp2->GetElem(4,1))));

Memo1->Lines->Add(“ U_X = “ + FloatToStr((temp2->GetElem(5,1))));

Memo1->Lines->Add(“ U_Y = “ + FloatToStr((temp2->GetElem(6,1))));

Memo1->Lines->Add(“”);

ProgressBar1->StepIt();

}

else

{

Timer1->Enabled = false;

File.close();

delete temp1;

delete temp2;

}

}

//---------------------------------------------------------------------------

Test.h

//---------------------------------------------------------------------------

#ifndef TestH

#define TestH

//---------------------------------------------------------------------------

#include <Classes.hpp>

#include <Controls.hpp>

#include <StdCtrls.hpp>

#include <Forms.hpp>

#include <Menus.hpp>

#include <Buttons.hpp>

#include <ExtCtrls.hpp>

#include <ComCtrls.hpp>

#include <fstream.h>

#include "MyMain.h"

//---------------------------------------------------------------------------

class TForm1 : public TForm

{

__published:// IDE-managed Components

TMainMenu *MainMenu1;

TMenuItem *N1;

TMenuItem *N2;

TMenuItem *N3;

TMemo *Memo1;

TTimer *Timer1;

TGroupBox *GroupBox1;

TButton *Button1;

TButton *Button2;

TProgressBar *ProgressBar1;

TBevel *Bevel2;

void __fastcall Button1Click(TObject *Sender);

void __fastcall Timer1Timer(TObject *Sender);

private:

float RealX;

float RealY;

float OtclonX;

float OtclonY;

float RealSpeedX;

float RealSpeedY;

float SpeedVetraX;

float SpeedTechX;

float OtclonSpeedVetraX;

float OtclonSpeedTechX;

float SpeedVetraY;

float SpeedTechY;

float OtclonSpeedVetraY;

float OtclonSpeedTechY;

float NachTime;

float KonechTime;

float delta;

TRealFilter* RealFilter;

TAbstractFilter* AbstractFilter;

ofstream File;

public:// User declarations

__fastcall TForm1(TComponent* Owner);

};

//---------------------------------------------------------------------------

extern PACKAGE TForm1 *Form1;

//---------------------------------------------------------------------------

#endif

Графики





P[1,1]

P[2,2]



P[3,3]

P[4,4]

P[5,5]

P[6,6]

Литература

1) «Лекции по Статистической динамике» М.Н.Красильщиков

2) «Статистическая динамика и оптимизация управления летательных аппаратов» М.Н.Красильщиков

3) «Объектно-ориентированное проектирование» Г.Буч

4) «Язык программирования C++» Б.Страуструп

5) «Программирование в C++ Builder 6» А.Я.Архангельский

Размещено на Allbest.ru