Модель фильтра Калмана 2 порядка — различия между версиями
Материал из SRNS
Korogodin (обсуждение | вклад) (→Листинг) |
Ippolitov (обсуждение | вклад) |
||
(не показаны 18 промежуточных версий 1 участника) | |||
Строка 14: | Строка 14: | ||
|repository = | |repository = | ||
|category1 = Статистическая радиотехника | |category1 = Статистическая радиотехника | ||
− | |category2 = | + | |category2 = Оценивание частоты |
− | |category3 = | + | |category3 = Оценивание задержки |
|category4 = | |category4 = | ||
− | |category5 = | + | |category5 = |
|category6 = | |category6 = | ||
|category7 = | |category7 = | ||
Строка 61: | Строка 61: | ||
for k = 1:K | for k = 1:K | ||
Ud = f(Xextr, Xist); % Дискриминатор | Ud = f(Xextr, Xist); % Дискриминатор | ||
− | Sd = f(A_IQ); % | + | Sd = f(A_IQ); % Крутизна дискриминационной характеристики |
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на k-й интервал | Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на k-й интервал | ||
Xextr = F*Xest; % Экстраполяция на интервал k+1 | Xextr = F*Xest; % Экстраполяция на интервал k+1 | ||
Строка 70: | Строка 70: | ||
=== С моделью фазы === | === С моделью фазы === | ||
+ | |||
+ | Для моделирования ЧАП с статистическими эквивалентами корреляторов необходимо учитывать разность набегов фазы приходящего сигнала и фазы опорного колебания в корреляторе. Тогда удобно использовать следующую модель: | ||
<source lang="matlab"> | <source lang="matlab"> | ||
Строка 79: | Строка 81: | ||
qcno_ist = 45*ones(1,K); % // SNR | qcno_ist = 45*ones(1,K); % // SNR | ||
− | H = | + | H = 3; % Hz, полоса |
Xextr = [0; 0; 0]; % Вектор экстраполяций | Xextr = [0; 0; 0]; % Вектор экстраполяций | ||
Строка 98: | Строка 100: | ||
Ko(3) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме | Ko(3) = 2*16/9*H^2; % Коэффициенты непрерывной системы в установившемся режиме | ||
Ko(2) = sqrt(2*Ko(3)); | Ko(2) = sqrt(2*Ko(3)); | ||
− | Ko(1) = 0; | + | Ko(1) = 0; % Это не баг, это фича! Из-за этого нуля система, на самом деле, - второго порядка |
Ko = Ko*Tf; % Переход к коэффициентам дискретной системы | Ko = Ko*Tf; % Переход к коэффициентам дискретной системы | ||
Строка 105: | Строка 107: | ||
stdIst = 10; nIst = randn(1,K); | stdIst = 10; nIst = randn(1,K); | ||
− | stdn_IQ = ones(1,K)*8; | + | stdn_IQ = ones(1,K)*8; % СКО шума квадратурных сумм |
A_IQ = nan(1,K); % // Memory allocation | A_IQ = nan(1,K); % // Memory allocation | ||
A_IQ_eff = nan(1,K); | A_IQ_eff = nan(1,K); | ||
− | I = nan(1,K) | + | I = nan(1,K); % // Memory allocation |
− | Q = nan(1,K) | + | Q = nan(1,K); |
− | EpsPhi = nan(1, K); | + | EpsPhi = nan(1, K); % // Memory allocation |
EpsW = nan(1, K); | EpsW = nan(1, K); | ||
EpsTau = nan(1, K); | EpsTau = nan(1, K); | ||
− | nI = stdn_IQ | + | nI = stdn_IQ.*randn(1,K); % // I-comp noise |
− | nQ = stdn_IQ | + | nQ = stdn_IQ.*randn(1,K); % // Q-comp noise |
+ | w = 0; Isum = 0; Qsum = 0; Iold = 1; Qold = 0; | ||
for k = 1:K | for k = 1:K | ||
− | + | ||
+ | % // Расчет стат.эквивалентов корреляционных сумм | ||
EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi); | EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi); | ||
EpsW(k) = Xist(2) - Xextr(2); | EpsW(k) = Xist(2) - Xextr(2); | ||
EpsTau(k) = 0; | EpsTau(k) = 0; | ||
− | + | ||
− | + | qcno = 10.^(qcno_ist(k)/10); | |
+ | A_IQ(k) = stdn_IQ(k) .* sqrt(2 * qcno * Tc); | ||
+ | |||
A_IQ_eff(k) = A_IQ(k)*sinc(EpsW(k)*Tc/2 /pi)*ro(EpsTau(k)); | 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)); | mI = A_IQ_eff(k) * cos(EpsW(k)*Tc/2 + EpsPhi(k)); | ||
Строка 132: | Строка 138: | ||
I(k) = mI + nI(k); | I(k) = mI + nI(k); | ||
Q(k) = mQ + nQ(k); | Q(k) = mQ + nQ(k); | ||
− | + | Isum = Isum + I(k); | |
+ | Qsum = Qsum + Q(k); | ||
+ | |||
Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления | Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления | ||
− | Ud = f( | + | w = w + 1; |
− | Sd = f(A_IQ); | + | if w == fix(Tf/Tc) |
− | + | % Ud = f(I(k), Q(k), I(k-1), Q(k-1), ...); % Дискриминатор | |
− | + | Ud = (I(k)*Qold - Q(k)*Iold); | |
+ | % Sd = f(A_IQ); % Крутизна дискриминационной характеристики | ||
+ | Sd = Tc*(A_IQ(k)*Tf/Tc)^2 * 1.3; | ||
+ | Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на очередной интервал фильтра | ||
+ | Xextr = Ff*Xest; % Экстраполяция на следующий интервал | ||
+ | w = 0; | ||
+ | Iold = Isum; Isum = 0; | ||
+ | Qold = Qsum; Qsum = 0; | ||
+ | end | ||
− | Xist = Fc*Xist + [0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния | + | Xist = Fc*Xist + [0; 0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния |
+ | if ~mod(k,fix(K/10)) | ||
+ | fprintf('Progress: %.0f%%\n', 100*k/K); | ||
+ | end | ||
end | end | ||
+ | |||
</source> | </source> | ||
== Результаты моделирования при параметрах по-умолчанию == | == Результаты моделирования при параметрах по-умолчанию == | ||
+ | |||
+ | <gallery> | ||
+ | Image:20111012_FLL_1.png|Ошибка слежения за частотой для ЧАП из примера | ||
+ | </gallery> | ||
== См. также == | == См. также == |
Текущая версия на 17:05, 22 июля 2016
![]() | |
---|---|
Описание | Модель фильтра Калмана 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 = 3; % 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); % // Memory allocation
Q = nan(1,K);
EpsPhi = nan(1, K); % // Memory allocation
EpsW = nan(1, K);
EpsTau = nan(1, K);
nI = stdn_IQ.*randn(1,K); % // I-comp noise
nQ = stdn_IQ.*randn(1,K); % // Q-comp noise
w = 0; Isum = 0; Qsum = 0; Iold = 1; Qold = 0;
for k = 1:K
% // Расчет стат.эквивалентов корреляционных сумм
EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi);
EpsW(k) = Xist(2) - Xextr(2);
EpsTau(k) = 0;
qcno = 10.^(qcno_ist(k)/10);
A_IQ(k) = stdn_IQ(k) .* sqrt(2 * qcno * 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);
Isum = Isum + I(k);
Qsum = Qsum + Q(k);
Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления
w = w + 1;
if w == fix(Tf/Tc)
% Ud = f(I(k), Q(k), I(k-1), Q(k-1), ...); % Дискриминатор
Ud = (I(k)*Qold - Q(k)*Iold);
% Sd = f(A_IQ); % Крутизна дискриминационной характеристики
Sd = Tc*(A_IQ(k)*Tf/Tc)^2 * 1.3;
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на очередной интервал фильтра
Xextr = Ff*Xest; % Экстраполяция на следующий интервал
w = 0;
Iold = Isum; Isum = 0;
Qold = Qsum; Qsum = 0;
end
Xist = Fc*Xist + [0; 0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния
if ~mod(k,fix(K/10))
fprintf('Progress: %.0f%%\n', 100*k/K);
end
end
Tf = 0.005; % Период работы фильтров
Tc = 0.001; % Период интегрирования в корреляторе
K = fix(Tmod/Tc);
qcno_ist = 45*ones(1,K); % // SNR
H = 3; % 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); % // Memory allocation
Q = nan(1,K);
EpsPhi = nan(1, K); % // Memory allocation
EpsW = nan(1, K);
EpsTau = nan(1, K);
nI = stdn_IQ.*randn(1,K); % // I-comp noise
nQ = stdn_IQ.*randn(1,K); % // Q-comp noise
w = 0; Isum = 0; Qsum = 0; Iold = 1; Qold = 0;
for k = 1:K
% // Расчет стат.эквивалентов корреляционных сумм
EpsPhi(k) = mod(Xist(1) - Xextr(1),2*pi);
EpsW(k) = Xist(2) - Xextr(2);
EpsTau(k) = 0;
qcno = 10.^(qcno_ist(k)/10);
A_IQ(k) = stdn_IQ(k) .* sqrt(2 * qcno * 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);
Isum = Isum + I(k);
Qsum = Qsum + Q(k);
Xextr = Fincorr * Xextr; % Набег фазы в корреляторе к концу накопления
w = w + 1;
if w == fix(Tf/Tc)
% Ud = f(I(k), Q(k), I(k-1), Q(k-1), ...); % Дискриминатор
Ud = (I(k)*Qold - Q(k)*Iold);
% Sd = f(A_IQ); % Крутизна дискриминационной характеристики
Sd = Tc*(A_IQ(k)*Tf/Tc)^2 * 1.3;
Xest = Xextr + Ko*Ud/Sd; % Вектор оценок на очередной интервал фильтра
Xextr = Ff*Xest; % Экстраполяция на следующий интервал
w = 0;
Iold = Isum; Isum = 0;
Qold = Qsum; Qsum = 0;
end
Xist = Fc*Xist + [0; 0; 1]*nIst(k)*stdIst; % Здесь может быть любая другая модель изменения истинного вектора состояния
if ~mod(k,fix(K/10))
fprintf('Progress: %.0f%%\n', 100*k/K);
end
end