Модель фильтра Калмана 2 порядка — различия между версиями
Материал из SRNS
Korogodin (обсуждение | вклад) (→Листинг) |
Korogodin (обсуждение | вклад) (→Листинг) |
||
Строка 36: | Строка 36: | ||
[[File:20110520_K2K3.png|thumb|Коэффициенты передачи непрерывных следящих систем второго и третьего порядка]] | [[File:20110520_K2K3.png|thumb|Коэффициенты передачи непрерывных следящих систем второго и третьего порядка]] | ||
+ | |||
+ | === Без моделирования фазы (первообразной от главного оцениваемого параметра) === | ||
<source lang="matlab"> | <source lang="matlab"> | ||
Строка 64: | Строка 66: | ||
Xist = F*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния | Xist = F*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния | ||
+ | end | ||
+ | </source> | ||
+ | |||
+ | === С моделью фазы === | ||
+ | |||
+ | <source lang="matlab"> | ||
+ | Tmod = 300; % Время моделирования | ||
+ | Tf = 0.005; % Период работы фильтров | ||
+ | Tc = 0.001; % Период интегрирования в корреляторе | ||
+ | K = fix(Tmod/Tc); | ||
+ | |||
+ | qcno_ist = 45*ones(1,K); % // SNR | ||
+ | |||
+ | H = 20; % Hz, полоса | ||
+ | |||
+ | Xextr = [0; 0; 0]; % Вектор экстраполяций | ||
+ | |||
+ | Ff = [1 0 0 | ||
+ | 0 1 Tf | ||
+ | 0 0 1]; % Переходная матрица для модели частоты (в темпе фильтра) | ||
+ | |||
+ | Fc = [1 Tc Tc^2/2 | ||
+ | 0 1 Tc | ||
+ | 0 0 1]; % Переходная матрица для модели частоты (в темпе коррелятора) | ||
+ | |||
+ | Fincorr = [1 Tc 0 | ||
+ | 0 1 0 | ||
+ | 0 0 1]; % Переходная матрица набега фазы в корреляторе | ||
+ | |||
+ | Ko = nan(3,1); % Вектор-столбец коэффициентов фильтра | ||
+ | Ko(3) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме | ||
+ | Ko(2) = sqrt(2*Ko(3)); | ||
+ | Ko(1) = 0; | ||
+ | |||
+ | Ko = Ko*Tf; % Переход к коэффициентам дискретной системы | ||
+ | |||
+ | Xist = [0; 0; 0]; % Истинный вектор состояния | ||
+ | stdIst = 10; nIst = randn(1,K); | ||
+ | |||
+ | stdn_IQ = ones(1,K)*8; | ||
+ | |||
+ | A_IQ = nan(1,K); % // Memory allocation | ||
+ | A_IQ_eff = nan(1,K); | ||
+ | |||
+ | I = nan(1,K); Iold = 1; % // Memory allocation | ||
+ | Q = nan(1,K); Qold = 1; | ||
+ | |||
+ | EpsPhi = nan(1, K); | ||
+ | EpsW = nan(1, K); | ||
+ | EpsTau = nan(1, K); | ||
+ | |||
+ | nI = stdn_IQ(k)*randn(1,K); % // I-comp noise | ||
+ | nQ = stdn_IQ(k)*randn(1,K); % // Q-comp noise | ||
+ | |||
+ | for k = 1:K | ||
+ | |||
+ | EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi); | ||
+ | EpsW(k) = Xist(2) - Xextr(2); | ||
+ | EpsTau(k) = 0; | ||
+ | |||
+ | [A_IQ(k) qcno] = qcno_change(qcno_ist(k), stdn_IQ(k), Tc); | ||
+ | A_IQ_eff(k) = A_IQ(k)*sinc(EpsW(k)*Tc/2 /pi)*ro(EpsTau(k)); | ||
+ | mI = A_IQ_eff(k) * cos(EpsW(k)*Tc/2 + EpsPhi(k)); | ||
+ | mQ = - A_IQ_eff(k) * sin(EpsW(k)*Tc/2 + EpsPhi(k)); | ||
+ | I(k) = mI + nI(k); | ||
+ | Q(k) = mQ + nQ(k); | ||
+ | |||
+ | Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления | ||
+ | |||
+ | Ud = f(Xextr, Xist); % Дискриминатор | ||
+ | Sd = f(A_IQ); % Критизна дискриминационной характеристики | ||
+ | Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на очередной интервал фильтра | ||
+ | Xextr = Ff*Xest; % Экстраполяция на следующий интервал | ||
+ | |||
+ | Xist = Fc*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния | ||
end | end | ||
</source> | </source> |
Версия 15:19, 12 октября 2011
Модель фильтра Калмана 2 порядка | |
---|---|
Описание | Модель фильтра Калмана 2 порядка на примере ФАП |
Автор(ы) | Korogodin (Korogodinобсуждение) |
Последняя версия | 1.0 (12.10.2011) |
Загрузить | no link |
Хранилище | no link |
Категории | Статистическая радиотехника, Фазовые измерения, Переходные процессы |
Содержание |
Описание модели
Модель фильтра Калмана 2 порядка, например, используемого в ЧАП. В данный момент приведен листинг только для коэффициентов установившегося режима. Следует привести пример с уравнениями Рикатти.
Листинг
Ниже приведен листинг при использовании коэффициентов установившегося режима. Изложение следует дополнить уравнениями Рикатти - для честного соответствия заголовку.
Без моделирования фазы (первообразной от главного оцениваемого параметра)
Tmod = 300; % Время моделирования
Tc = 0.001; % Период работы фильтров
K = fix(Tmod/Tc);
Xextr = [0; 0]; % Вектор экстраполяций
F = [1 Tc
0 1 ]; % Переходная матрица
H = 20; % Hz, полоса
Ko = nan(2,1); % Вектор-столбец коэффициентов фильтра
Ko(2) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме
Ko(1) = sqrt(2*Ko(2));
Ko = Ko*Tc; % Переход к коэффициентам дискретной системы
Xist = [0; 0]; % Истинный вектор состояния
stdIst = 10; nIst = randn(1,K);
for k = 1:K
Ud = f(Xextr, Xist); % Дискриминатор
Sd = f(A_IQ); % Критизна дискриминационной характеристики
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на k-й интервал
Xextr = F*Xest; % Экстраполяция на интервал k+1
Xist = F*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния
end
Tc = 0.001; % Период работы фильтров
K = fix(Tmod/Tc);
Xextr = [0; 0]; % Вектор экстраполяций
F = [1 Tc
0 1 ]; % Переходная матрица
H = 20; % Hz, полоса
Ko = nan(2,1); % Вектор-столбец коэффициентов фильтра
Ko(2) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме
Ko(1) = sqrt(2*Ko(2));
Ko = Ko*Tc; % Переход к коэффициентам дискретной системы
Xist = [0; 0]; % Истинный вектор состояния
stdIst = 10; nIst = randn(1,K);
for k = 1:K
Ud = f(Xextr, Xist); % Дискриминатор
Sd = f(A_IQ); % Критизна дискриминационной характеристики
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на k-й интервал
Xextr = F*Xest; % Экстраполяция на интервал k+1
Xist = F*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния
end
С моделью фазы
Tmod = 300; % Время моделирования
Tf = 0.005; % Период работы фильтров
Tc = 0.001; % Период интегрирования в корреляторе
K = fix(Tmod/Tc);
qcno_ist = 45*ones(1,K); % // SNR
H = 20; % Hz, полоса
Xextr = [0; 0; 0]; % Вектор экстраполяций
Ff = [1 0 0
0 1 Tf
0 0 1]; % Переходная матрица для модели частоты (в темпе фильтра)
Fc = [1 Tc Tc^2/2
0 1 Tc
0 0 1]; % Переходная матрица для модели частоты (в темпе коррелятора)
Fincorr = [1 Tc 0
0 1 0
0 0 1]; % Переходная матрица набега фазы в корреляторе
Ko = nan(3,1); % Вектор-столбец коэффициентов фильтра
Ko(3) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме
Ko(2) = sqrt(2*Ko(3));
Ko(1) = 0;
Ko = Ko*Tf; % Переход к коэффициентам дискретной системы
Xist = [0; 0; 0]; % Истинный вектор состояния
stdIst = 10; nIst = randn(1,K);
stdn_IQ = ones(1,K)*8;
A_IQ = nan(1,K); % // Memory allocation
A_IQ_eff = nan(1,K);
I = nan(1,K); Iold = 1; % // Memory allocation
Q = nan(1,K); Qold = 1;
EpsPhi = nan(1, K);
EpsW = nan(1, K);
EpsTau = nan(1, K);
nI = stdn_IQ(k)*randn(1,K); % // I-comp noise
nQ = stdn_IQ(k)*randn(1,K); % // Q-comp noise
for k = 1:K
EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi);
EpsW(k) = Xist(2) - Xextr(2);
EpsTau(k) = 0;
[A_IQ(k) qcno] = qcno_change(qcno_ist(k), stdn_IQ(k), Tc);
A_IQ_eff(k) = A_IQ(k)*sinc(EpsW(k)*Tc/2 /pi)*ro(EpsTau(k));
mI = A_IQ_eff(k) * cos(EpsW(k)*Tc/2 + EpsPhi(k));
mQ = - A_IQ_eff(k) * sin(EpsW(k)*Tc/2 + EpsPhi(k));
I(k) = mI + nI(k);
Q(k) = mQ + nQ(k);
Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления
Ud = f(Xextr, Xist); % Дискриминатор
Sd = f(A_IQ); % Критизна дискриминационной характеристики
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на очередной интервал фильтра
Xextr = Ff*Xest; % Экстраполяция на следующий интервал
Xist = Fc*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния
end
Tf = 0.005; % Период работы фильтров
Tc = 0.001; % Период интегрирования в корреляторе
K = fix(Tmod/Tc);
qcno_ist = 45*ones(1,K); % // SNR
H = 20; % Hz, полоса
Xextr = [0; 0; 0]; % Вектор экстраполяций
Ff = [1 0 0
0 1 Tf
0 0 1]; % Переходная матрица для модели частоты (в темпе фильтра)
Fc = [1 Tc Tc^2/2
0 1 Tc
0 0 1]; % Переходная матрица для модели частоты (в темпе коррелятора)
Fincorr = [1 Tc 0
0 1 0
0 0 1]; % Переходная матрица набега фазы в корреляторе
Ko = nan(3,1); % Вектор-столбец коэффициентов фильтра
Ko(3) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме
Ko(2) = sqrt(2*Ko(3));
Ko(1) = 0;
Ko = Ko*Tf; % Переход к коэффициентам дискретной системы
Xist = [0; 0; 0]; % Истинный вектор состояния
stdIst = 10; nIst = randn(1,K);
stdn_IQ = ones(1,K)*8;
A_IQ = nan(1,K); % // Memory allocation
A_IQ_eff = nan(1,K);
I = nan(1,K); Iold = 1; % // Memory allocation
Q = nan(1,K); Qold = 1;
EpsPhi = nan(1, K);
EpsW = nan(1, K);
EpsTau = nan(1, K);
nI = stdn_IQ(k)*randn(1,K); % // I-comp noise
nQ = stdn_IQ(k)*randn(1,K); % // Q-comp noise
for k = 1:K
EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi);
EpsW(k) = Xist(2) - Xextr(2);
EpsTau(k) = 0;
[A_IQ(k) qcno] = qcno_change(qcno_ist(k), stdn_IQ(k), Tc);
A_IQ_eff(k) = A_IQ(k)*sinc(EpsW(k)*Tc/2 /pi)*ro(EpsTau(k));
mI = A_IQ_eff(k) * cos(EpsW(k)*Tc/2 + EpsPhi(k));
mQ = - A_IQ_eff(k) * sin(EpsW(k)*Tc/2 + EpsPhi(k));
I(k) = mI + nI(k);
Q(k) = mQ + nQ(k);
Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления
Ud = f(Xextr, Xist); % Дискриминатор
Sd = f(A_IQ); % Критизна дискриминационной характеристики
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на очередной интервал фильтра
Xextr = Ff*Xest; % Экстраполяция на следующий интервал
Xist = Fc*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния
end