状态空间模型
狀態空間模型是動態時域模型，以隱含著的時間為自變量。狀態空間模型包括兩個模型：
一是狀態方程模型，反映動態系統在輸入變量作用下在某時刻所轉移到的狀態；
二是輸出或量測方程模型，它將系統在某時刻的輸出和系統的狀態及輸入變量聯系起來。
狀態空間模型分類
??狀態空間模型按所受影響因素的不同分為：
（1）確定性狀態空間模型
（2）隨機性狀態空間模型
??狀態空間模型按數值形式分為：
（1）離散空間狀態模型
（2）連續空間狀態模型
狀態空間模型按所描述的動態系統可以分為：
（1）線性的與非線性的
（2）時變的與時不變的
5.jpg (105.47 KB, 下載次數: 15)
下載附件
二、系統的狀態空間
離散事件隨機性系統的概念是系統理論中最基本的概念。
離散事件隨機性系統的狀態，是指系統內部的可能運動狀態和可能儲能狀態。系統在k=k0時刻的狀態，是在k<k0時以系統內部儲能的積累結果，并在k=k0時以系統要素儲能的方式表現出來，還將影響系統在k>k0時的外部行為。
用隨機向量序列來描述系統在任一時刻的狀態向量，稱為狀態向量法，也稱為狀態空間法。狀態向量表示為：
6.jpg (10.91 KB, 下載次數: 15)
下載附件
其中
7.jpg (6.88 KB, 下載次數: 14)
下載附件
（k=1，2，1…，n）為第i個狀態向量。三、系統的輸入輸出
輸入輸出狀態概念
引入狀態向量是為了對系統內部結構進行數學描述，但在許多情況下，系統的狀態是無法直接量測到的，有時甚至全部不能量測到。在實際工作中，能量測到的量之勢系統的輸入與輸出。
輸入輸出變量表示
系統的輸入也是隨時間而變的一組變量，表示為：
8.jpg (9.92 KB, 下載次數: 13)
下載附件
稱為輸入向量，其分量
9.jpg (8.07 KB, 下載次數: 12)
下載附件
（i=1，2，…，r)稱為輸入變量。系統所受的隨機干擾也是隨時間而變的一組變量，表示為
10.jpg (11.01 KB, 下載次數: 15)
下載附件
稱為系統的動態模型噪聲，它是系統的一種特殊輸入向量。系統的輸出也是隨時間而變的一組變量，表示為：
11.jpg (9.29 KB, 下載次數: 11)
下載附件
稱為輸出向量，其分量? ???（i=1，2，…，m)稱為輸入變量。量測系統也會受到隨機噪聲的污染，表示為：
12.jpg (8.93 KB, 下載次數: 15)
下載附件
稱為系統的量測噪聲。
四、狀態空間模型
狀態空間模型定義
狀態空間模型是描述動態系統的完整模型，它表達了由于輸入引起系統內部狀態的變化，并由此使輸出發生的變化。
狀態空間模型的不同形式
如，線性時不變模型的狀態方程可表示為：
13.jpg (5.84 KB, 下載次數: 10)
下載附件
輸出方程為：
14.jpg (3.36 KB, 下載次數: 12)
下載附件
五、狀態空間模型的建立
? 例 1? ? 某養魚場為了反映池塘魚種的變化，請你幫助建立狀態空間模型。
解答： （1）取狀態向量X（k）為k時刻3個魚種的數量：
15.jpg (19.19 KB, 下載次數: 13)
下載附件
輸入向量為：16.jpg (25.87 KB, 下載次數: 14)
下載附件
（2）狀態轉移矩陣
17.jpg (8.44 KB, 下載次數: 15)
下載附件
式中： p1，p2，p3為鯽魚、青魚和鯉魚的生長率，這里為p1=0.1，p2=0.13，p3=0.08。（3）輸入矩陣仍定為常陣
18.jpg (8.26 KB, 下載次數: 11)
下載附件
（4）輸出矩陣或預測矩陣C為3×3維單位陣，這樣輸出向量或量測向量就等同于狀態向量，狀態空間模型：
19.jpg (7.91 KB, 下載次數: 13)
下載附件
即：
20.jpg (33.42 KB, 下載次數: 12)
下載附件
21.jpg (4.77 KB, 下載次數: 14)
下載附件
22.jpg (13.19 KB, 下載次數: 11)
下載附件
Kalman濾波簡介
Kalman濾波是一種線性濾波與預測方法，原文為：A New Approach to Linear Filtering and Prediction Problems。文章推導很復雜，看了一半就看不下去了，既然不能透徹理解其原理，但總可以通過實驗來理解其具體的使用方法。
Kalman濾波分為2個步驟，預測(predict)和校正(correct)。預測是基于上一時刻狀態估計當前時刻狀態，而校正則是綜合當前時刻的估計狀態與觀測狀態，估計出最優的狀態。預測與校正的過程如下：
預測：
1.png (6.58 KB, 下載次數: 12)
下載附件
校正：
2.png (11.75 KB, 下載次數: 15)
下載附件
公式1是狀態預測，公式2是誤差矩陣預測，公式3是kalman增益計算，公式4是狀態校正，其輸出即是最終的kalman濾波結果，公式5是誤差矩陣更新。各變量說明如下表：
4.jpg (120.25 KB, 下載次數: 12)
下載附件
算法實現與分析
Kalman濾波最復雜的計算應該就是公式3中的矩陣求逆，考慮到實現的方便性，采用matlab來簡單實現，本文主要是分析kalman濾波中各個變量的作用和對濾波結果的影響。具體代碼如下：
| 1 2 3 4 5 6 7 8 9 | function filter = Kalman(filter) ??%predict ??predict_x = filter.A * filter.x + filter.B * filter.u; ??filter.P = filter.A * filter.P * filter.A' + filter.Q; ??%correct ??filter.K = filter.P * filter.H' / (filter.H * filter.P * filter.H' + filter.R); ??filter.x = predict_x + filter.K * (filter.z - filter.H * predict_x); ??filter.P = filter.P - filter.K * filter.H * filter.P; end |
在matlab中，kalman濾波實際上就是上面那5個公式，而難點卻是在測試代碼中針對不同問題各個變量的初始化上，下面來逐個分析。
1.建立模型，明確觀測量，系統狀態以及其轉移方程(下面這段公式太多，通過word寫好后截圖)
3.png (121.08 KB, 下載次數: 13)
下載附件
2.初始化噪聲協方差矩陣
經過上面一步，只有PQRK四個矩陣還未確定了。顯然增益矩陣K是不需要初始化的，P是誤差矩陣，初始化可以是一個隨機的矩陣或者0，只要經過幾次的處理基本上就能調整到正常的水平，因此也就只會影響前面幾次的濾波結果。
Q和R分別是預測和觀測狀態協方差矩陣，一般可以簡單認為系統狀態各維之間(即上面的a和b)相互獨立，那么Q和R就可以設置為對角陣。而這兩個對角線元素的大小將直接影響著濾波結果，若Q的元素遠大于R的元素，則預測噪聲大，從而更相信觀測值，這樣可能使得kalman濾波結果與觀測值基本一致；反之，則更相信預測，kalman濾波結果會表現得比較規整和平滑；若二者接近，則濾波結果介于前面兩者之間，根據實驗效果看也缺乏實際使用價值。
以上幾個矩陣確定后，對于狀態x，由于0時刻我們沒有任何關于該系統的知識，可以使用0時刻的測量值z0來初始x0，預測從k=1開始；也可以初始化-1時刻的狀態，當然這個狀態實際是未知的，也就可隨機取。2種方式都可以，但使用0時刻測量值來初始化狀態，可以使得前面幾次預測更準確。
3.實驗分析
首先使用下面代碼生成一組數據存在z.mat中：
[C] 純文本查看 復制代碼 ?| 1 2 3 4 5 6 7 8 | interval = pi/18; t = 1:interval:100*pi; len = size(t, 2); a = t + 4 * (rand(1,len)-0.5); b = t .* sin(t/10) +? 10 * (rand(1,len)-0.5); z = [a; b]; save('z.mat','z'); plot(z(1,:),z(2,:),'o') |
可以看出其近似為一條振幅不斷增大的正弦曲線疊加一個隨機噪聲。繪制出來如下：
4.png (23.9 KB, 下載次數: 10)
下載附件
如果使用上面推導的恒定狀態系統模型，代碼與實驗結果如下：
[C] 純文本查看 復制代碼 ?| 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 | clear close all clc dim_observe = 2;????????? %觀測值維數 n = dim_observe;? %狀態維數，觀測狀態每個維度都有1個速度，故需乘2 filter.A = eye(n);%[1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1]; filter.B = 0; filter.u = 0; filter.P = eye(n); filter.K = zeros(n); filter.H = eye(n);%[1,0,0,0;0,1,0,0]; cQ = 1e-8; cR = 1e-2; filter.Q = eye(n) * cQ;??????? %這里簡單設置Q和R對角線元素都相等，設為不等亦可 filter.R = eye(dim_observe) * cR; filter.x = zeros(n,1); %初始狀態x0 load('z.mat'); figure(1),subplot(2,2,1), t = 1; out = []; for i=1:size(z,2) ??filter.z = z(:,i); ??filter = Kalman(filter); ??plot(filter.x(1),filter.x(2),? 'r*');hold on??????? ??plot(filter.z(1),filter.z(2),? 'bo');??????? hold on ??out=[out filter.x]; %???????? pause(.5) end figure(1), str = sprintf('cQ = %e, cR = %e', cQ, cR); title(str) %畫局部放大 subplot(2,2,2), plot(out(1,:),out(2,:),? 'r*');hold on??????? plot(z(1,:),z(2,:),? 'bo');??????? hold on axis([120 170 80 200]) |
5.png (32.82 KB, 下載次數: 13)
下載附件
可以看出濾波結果完全滯后于測量數據，其根本原因在于建立的模型存在問題。
如果采用上面推導的物體運動模型則只需要修改部分代碼，主要是矩陣A和H，以及其他矩陣對應的維數，具體如下：
[C] 純文本查看 復制代碼 ?| 1 2 3 4 5 6 7 8 | dim_observe = 2;????? %觀測值維數 n = 2 * dim_observe;? %狀態維數，觀測狀態每個維度都有1個速度，故需乘2 filter.A = [1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1]; filter.B = 0; filter.u = 0; filter.P = eye(n); filter.K = zeros(n); filter.H = [1,0,0,0;0,1,0,0]; |
運行結果如下圖，藍色為觀測數據，紅色為kalman濾波數據，右側為局部放大圖。可以看出經過濾波后的數據相當平滑，這里Q和R中元素的量級分別為cQ和cR，下圖結果可以看到cR比cQ多了6個數量級。
6.png (39.06 KB, 下載次數: 14)
下載附件
（1）
增加幾組結果用于對比分析，對于的cQ和cR見圖的標題。
7.png (39.18 KB, 下載次數: 14)
下載附件
（2）
8.png (40.65 KB, 下載次數: 13)
下載附件
（3）
9.png (41.63 KB, 下載次數: 14)
下載附件
（4）
10.png (41.2 KB, 下載次數: 13)
下載附件
（5）
11.png (41.14 KB, 下載次數: 11)
下載附件
（6）
首先看圖1和2，cR與cQ大小均相差了3個數量級，而二者的比值相同，則kalman濾波結果相同。
再看圖2~圖6，cR/cQ在不斷減小，kalman濾波結果的平滑性也在不斷降低，到圖5和6中，濾波結果完全和觀測值相同，說明此時kalman濾波已經完全相信觀測值了。原因在于cR/cQ過小，系統認為預測噪聲的方差很大，不值得信賴，而觀測值的噪聲方差小，可信度高。
根據上面的實驗結果，可以看出Kalman濾波應用中的幾個問題：
1.模型建立的正確性從根本上決定了濾波效果的正確性。
上面使用物體靜止模型進行濾波，結果完全不對，而使用勻速運動模型則能達到較好的效果。從根本上講，上面的數據也不是勻速運動的，為何結果會基本正確？看看第一個使用靜止模型的濾波結果，雖然我們假定了物體是靜止的，但由于觀測數據的作用，kalman濾波結果也會有相應的運動而不是完全靜止，也就是說濾波器在不停地修正這個狀態，而在勻速運動模型中，物體的速度我們認為不變，但同樣地kalman濾波器也會不停地修正這個速度，濾波器中計算的速度實質的偏離了真實速度的，因此最終也會有相應的偏差，不過這個偏差在我們容許范圍內，也就可以大膽使用了。
如果能確定物體是勻變速直線運動，使用相應帶加速度的模型會得到更準確的效果。但是越嚴格的模型其適用范圍也相應越小。
2.影響濾波結果平滑性的因素是cR/cQ，這個值反映了我們對于預測和觀測值的信任程度；其值越大則越相信預測結果，濾波結果平滑性好；反之則越相信觀測結果，濾波結果越偏向于觀測值。一般我們使用kalman濾波器是為了能平滑數據的波動，因此應盡量保證cR/cQ稍大，上面的測試結果該值在1e4以上數據較為平滑。
總結
- 上一篇： 如何用PADS打开AD的PCB文件？
- 下一篇： iOS录音功能